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ABSTRACT 


This  thesis  investigates  the  development  and  implementation  of  optimal  slew  trajectories 
for  positioning  a  spacecraft  antenna.  Conventional  maneuvers  are  developed  by 
considering  each  gimbal  independently.  Consequently,  maneuver  design  is  simple,  but 
may  be  highly  sub-optimal  and  cause  significant  torques  to  be  imposed  on  the  spacecraft 
body.  This  work  explores  the  impact  of  implementing  optimal  slew  paths  that  best  utilize 
system  dynamics  with  the  objective  of  increasing  available  customer  time  on 
communications  links  and  enabling  new  missions.  Accomplishing  this  required  the 
development  of  a  detailed  multibody  system  model  that  can  be  easily  tailored  to  any 
spacecraft  antenna  configuration.  Various  software  suites  were  used  to  perform  thorough 
validation  and  verification  of  the  Newton-Euler  formulation  developed  herein.  The 
antenna  model  was  then  utilized  to  solve  an  optimal  control  problem  for  a  geostationary 
communications  satellite.  The  developed  maneuvers  not  only  reduce  the  antenna  slew 
time,  but  also  reduce  the  impact  of  the  antenna  motion  on  the  spacecraft  attitude.  This 
reduces  reliance  on  the  spacecraft  attitude  control  system  to  maintain  pointing,  and 
minimizes  the  impact  of  antenna  motion  on  the  operation  of  other  payloads.  Successful 
implementation  of  the  designed  maneuvers  on  a  laboratory  testbed  validate  the  approach 
in  a  real  hardware  environment. 
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I.  INTRODUCTION 


A.  MOTIVATION 

Since  1983,  the  Tracking  and  Data  Relay  Satellite  System  (TDRSS)  constellation 
has  been  providing  Department  of  Defense  (DoD),  civil,  and  commercial  users  with 
communications  and  data  relay  services  for  a  variety  of  space-based  platforms  in  low 
earth  orbit  (LEO),  medium  earth  orbit  (MEO),  highly  elliptical  orbits  (HEO)  and  even 
geosynchronous  orbits  (GEO)  [1].  The  constellation  is  now  in  its  third  generation  with 
the  recent  launch  of  Tracking  and  Data  Relay  Satellite-K  (TDRS-K)  in  January,  2013  [2], 
The  current  constellation  consists  of  eight  satellites,  TDRS-D  through  TDRS-K,  as  seen 
in  Eigure  1. 
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Eigure  1.  TDRS  Constellation  (from  [3]) 


As  shown  in  Figure  2,  the  current  generation  of  TDRS  consists  of  four 
communications  links.  The  multiple  access  (MA)  antenna  provides  S-band  links  to  EEO 
users  in  a  ±14°  field-of-view  (FOV)  [4],  The  2.4  m  Space-Ground  Eink  Antenna  provides 
a  ground  link  to  the  White  Sands  Complex  (WSC)  or  Guam  Remote  Ground  Terminal 
(GRGT)  [5].  Forward  and  aft  omni  antennas  provide  S-band  Telemetry,  Tracking,  and 
Command  (TT&C)  [4].  Easily,  two  steerable  single  access  (SA)  antennas  provide  links  to 
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users  in  all  orbit  regimes  with  S-band,  Ku-band,  and  Ka-band  links  [4],  Optimization  of 
the  slewing  eontrol  for  these  4.9  m  dishes  is  the  focus  of  this  thesis. 
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Figure  2.  Third  Generation  TDRS  Overview  (from  [4]) 


B.  TDRS  SINGLE  ACCESS  ANTENNA  POINTING 

While  the  TDRSS  has  proven  to  be  a  very  effective  and  capable  constellation, 
there  are  limitations  to  its  ability  to  meet  all  customer  needs.  One  of  the  often  overlooked 
limitations  is  the  efficiency  of  the  SA  antenna  slews.  The  0.17°  beamwidth  of  the  SA 
antenna,  when  used  in  Ka-band,  drives  stringent  pointing  requirements  [6].  The  SA 
antenna  tracks  user  satellites  via  an  azimuth  and  elevation  gimbal  along  the  pitch  axis 
(South  direction)  and  roll  axis  (orbit  direction)  of  the  spacecraft,  as  shown  in  Figure  3. 
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Figure  3.  Third  Generation  TDRS  Exploded  View  (from  [7]) 


TDRS  operates  using  three  distinct  fields  of  view.  The  LEO  FOV  consists  of  a 
±14°  box  centered  on  nadir,  and  services  95%  of  all  TDRS  customers  [8].  The  primary 
field  of  view  covers  an  ellipse  where  the  major  axis  extends  ±32°  in  roll  (elevation),  and 
±24°  in  pitch  (azimuth)  [8].  Lastly,  an  extended  field  of  view  (EEOV)  extends  the 
primary  ellipse  out  to  72°  outboard,  allowing  for  the  servicing  of  highly  elliptical,  or  even 
geosynchronous  customers  [8]. 

In  any  operating  region,  when  a  contact  is  broken  with  a  departing  customer, 
however,  it  takes  considerable  time  for  the  antenna  to  slew  before  a  new  customer  can  be 
acquired.  While  a  nominal  slew  is  designed  to  take  three  minutes,  slews  can  take  well 
under  three  minutes  for  small  slews  within  LEO,  to  over  seven  minutes  for  a  large  slew  to 
or  from  the  outboard  limits  of  the  antenna  LOV  [8].  Unfortunately,  even  if  the  size  of  the 
slew  is  minimal  and  can  be  performed  under  three  minutes,  the  flight  software  inserts 
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dead  time  in  order  to  increase  the  slew  time  to  three  minutes  [8].  This  is  due  to  current 
limitations  with  the  scheduling  software. 

C.  TDRS  SCHEDULING  PROBLEM 

Presently,  the  contact  schedule  for  TDRSS  is  scheduled  three  weeks  in 
advance  [8].  Customers  submit  requests  for  contact,  including  desired  start  and  stop  times 
in  a  Schedule  Add  Request  (SAR)  [8,  9].  Scheduling  is  performed  using  a  combination  of 
three  systems,  including  the  Space  Network  Access  System  (SNAS),  as  shown  in  Figure 
4  [9].  Since  the  schedule  can  change  over  three  weeks  and  due  to  the  difficulty  of 
accurately  predicting  TDRS  and  target  satellite  ephemeris  three  weeks  in  advance,  a 
standardized  three  minute  block  of  time  is  scheduled  for  each  slew,  based  on  the 
maximum  slew  time  within  the  primary  FOV  [8].  If  the  antenna  slew  will  take  less  than 
three  minutes  to  complete,  an  appropriate  amount  of  dead  time  is  inserted  following 
cutoff  from  the  departing  customer  [8].  For  example,  if  a  slew  is  calculated  to  take  two 
minutes,  the  SA  antenna  will  remain  pointed  at  the  last  position  of  the  departing  satellite 
for  one  minute  before  beginning  the  two  minute  slew  maneuver  to  the  next  customer.  On 
completion  of  the  maneuver,  the  antenna  will  be  pointing  at  the  desired  position  and  be 
moving  at  the  rate  desired  to  intercept  the  new  satellite.  Slews  that  take  longer  than  three 
minutes  unexpectedly  (i.e.,  slight  increases  in  data  time  due  to  ephemeris  error  during 
initial  scheduling)  result  in  an  alarm  at  the  mission  operations  center  (MOC)  and  a 
reduction  in  user  operational  time.  Large  slews  that  are  known  ahead  of  time  to  exceed 
three  minutes  (a  seven  minute  slew  to  or  from  the  EFOV  for  the  Magnetospheric 
Multiscale  Mission  (MMS)  or  Radiation  Belt  Storm  Probes  (RBSP)  missions,  for 
example)  are  scheduled  in  advance  in  order  to  provide  the  customer  with  an  accurate  start 
time  [8]. 

The  scheduling  approach  outlined  above  may  lead  to  inefficient  use  of  the  TDRS 
system  and  even  result  in  dropped  users.  First,  there  is  the  obvious  inefficiency  of 
inserting  dead  time  for  small  slews.  While  there  may  in  fact  be  hours  of  dead  time 
between  contacts  due  to  a  lack  of  access  requests,  this  small  delay  could  greatly  impact 
customer  operational  time  and  satisfaction  during  periods  of  peak  congestion. 
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Furthermore,  it  may  be  possible  to  improve  on  larger  slews,  sueh  as  the  seven  minute 
EFOV  slews,  in  order  to  provide  more  operational  time.  Lastly,  due  to  the  seheduling 
eonstraints,  eertain  users  may  be  dropped  beeause  of  an  inability  to  meet  timeliness. 

One  sueh  example  of  this  type  of  dropped  mission  is  in  servieing  a  train  of  earth 
seienees  satellites.  Multiple  earth-seienees  satellites  are  plaeed  in  a  eoordinated  sun- 
synehronous  orbit  in  order  to  provide  eorrelated  passes  of  a  variety  of  data.  The  satellites 
fly  in  relatively  elose  proximity,  and  in  order  to  meet  their  requests,  the  SA  antenna  has 
to  slew  rapidly  to  eateh  eaeh  passing  satellite.  However,  sinee  the  maximum  time  to  slew 
between  eustomers  is  less  than  three  minutes,  the  eurrent  system  cannot  perform  this 
mission. 


Mission  Operations  Centers  (MOCs) 


UPS 


Figure  4.  TDRS  Scheduling  Process  (from  [9]) 


While  the  three-week  scheduling  constraint  limits  the  options  available  to 
improve  maneuver  efficiency,  certain  methods  can  be  explored  to  yield  measurable 
benefits.  Primarily,  if  it  can  be  shown  that  traditional  maneuvers  can  be  completed  in  less 
time  using  optimal  control  theory,  then  the  reduced-time  slews  can  be  implemented  to 
obtain  a  new  scheduled  slew  time,  which  can  replace  the  current  three  minute  slew. 
While  this  method  may  still  suffer  from  the  inefficiency  of  downtime  (due  to  the 
scheduling  problem),  improving  slew  time  will  increase  operational  availability. 
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particularly  during  periods  of  congestion.  Furthermore,  the  timeliness  of  the  largest  slews 
could  be  improved,  increasing  availability  for  manually  scheduled  slews  as  well.  Lastly, 
if  the  time  can  be  reduced  enough,  missions  like  the  Earth  seience  train  could  be  satisfied, 
thereby  further  extending  the  utility  of  the  TDRSS. 

D,  THESIS  OBJECTIVE  AND  SCOPE 

The  objective  of  this  thesis  is  to  apply  optimal  control  theory  to  improve  the 
design  of  spacecraft  antenna  slews.  The  goal  will  be  to  reduee  the  amount  of  time 
required  to  slew  between  customers.  It  was  also  found  during  preliminary  research  that 
the  slew  rate  during  program  track  (30  steps/s  or  0.225°/s)  was  the  most  limiting  factor  on 
antenna  slews  [10,  ll].i  It  was  assumed  that  this  limit  was  not  a  hard  limit  driven  by 
gimbal  design,  but  a  soft  limit  designed  to  ensure  that  the  impact  of  the  antenna  motion 
on  the  spaeecraft  body,  and  thus  the  other  communications  antennas,  is  minimal. 
Therefore,  another  objective  of  this  thesis  is  to  minimize  the  effect  of  antenna  slews  on 
the  accumulation  of  spacecraft  angular  momentum.  Improvements  to  this  area  could 
impact  a  wide  variety  of  areas  for  all  spacecraft  with  moving  appendages  or  systems, 
improving  the  momentum  space  of  control  systems,  increasing  pointing  accuracy,  and 
possibly  decreasing  fuel  and  mass  requirements.  The  result  of  applying  optimal  control 
theory  to  the  problem  of  antenna  slew  design  can  therefore  improve  the  availability  of 
these  systems  and  also  have  a  profound  affect  on  the  design  of  future  bus  systems. 

In  order  to  accomplish  optimization,  the  spacecraft  had  to  be  modeled.  The 
dynamics  of  the  TDRS  spacecraft  and  antenna  couple  can  be  described  succinctly  by 
Equation  (1.1)  and  Equation  (1.2),  where  h  is  the  inertia  dyadic  of  the  spacecraft,  la  is  the 
inertia  dyadic  of  the  SA  antenna,  Ra  is  the  influence  matrix  of  the  main  body  on  the 
antenna,  coa  and  coh  are  the  antenna  and  spacecraft  rotation  rates,  Hb  is  the  angular 
momentum  of  the  satellite,  and  Tt  is  torque  on  the  satellite,  Ta  is  torque  on  the  antenna. 
While  these  equations  can  be  expanded  in  order  to  include  sloshing  and  flexible  modes. 


1  It  is  assumed  that  the  rates  deseribed  in  [10],  whieh  are  given  in  steps  per  minute  and  degrees  per 
minute,  are  aetually  values  for  steps  per  seeond  and  degrees  per  seeond,  whieh  would  eorrelate  with  data 
from  [11]. 
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these  impacts  were  determined  to  be  outside  the  scope  of  this  thesis  and  assumed  to  be 
zero. 

I,(b,+Ra(b^+co,xH,=T,  (1.1) 

(1-2) 

Many  of  the  parameters  that  make  up  the  spacecraft  and  antenna  inertia  and  the 
influence  matrix  in  Equation  (1.1)  and  Equation  (1.2)  depend  on  the  configuration  of  the 
antenna  system.  Therefore,  Equation  (1.1)  and  Equation  (1.2)  cannot  be  used  directly  for 
analysis.  Instead  a  complete  dynamic  model  of  the  multibody  system  has  to  be  developed 
which  also  includes  the  influence  of  the  motion  of  the  spacecraft  body.  This  analysis  is 
outlined  in  Chapter  II  through  Chapter  IV.  Eurthermore,  the  model  was  validated  to 
ensure  accuracy  of  the  dynamic  response.  Validation  and  verification  of  the  dynamic 
model  is  described  in  Chapter  V. 

Eastly,  an  optimal  control  method  had  to  be  chosen.  The  tools  developed  at  the 
Naval  Postgraduate  School  (NPS)  for  attitude  maneuver  optimization  are  the  prime 
candidates  for  approaching  the  TDRS  slew  problem.  Eaculty  and  researchers  have 
explored  a  wide  variety  of  optimal  control  problems  and  have  had  particular  success  in 
improving  the  timeliness  and  efficiency  of  spacecraft  attitude  maneuvers.  In  2006,  the 
National  Aeronautics  and  Space  Administration  (NASA)  performed  a  zero-propellant 
maneuver  on  the  International  Space  Station  (ISS),  rotating  90°  and  180°  without  the  use 
of  propellant,  by  utilizing  a  maneuver  designed  at  NPS  [12].  In  2010,  NPS  was  given  the 
opportunity  to  perform  optimal  maneuvers  on  NASA’s  Transitional  Regional  and 
Coronal  Explorer  (TRACE)  spacecraft,  where  slew  times  were  improved  by 
approximately  20%  over  traditional  eigenaxis  maneuvers  [13]. 

These  optimal  maneuvers  were  developed  using  the  DIDO  software  designed  by 
NPS  professor  Dr.  Isaac  M.  Ross  [14].  This  software  will  be  utilized  here  to  develop 
optimal  spacecraft  antenna  slew  maneuvers.  Details  of  the  underlying  optimal  control 
theory  utilized  by  DIDO  and  how  DIDO  was  implemented  are  presented  in  Chapter  VI. 

Using  the  developed  TDRS  model,  slew  scenarios  were  created  for  use  as  optimal 

control  problems.  Each  scenario  was  solved  using  the  DIDO  software  to  generate  both 
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minimum-time,  and  minimum-momentum  slews.  The  designed  minimum  time 
trajeetories  were  implemented  on  a  laboratory  testbed  to  llustrate  an  approach  by  which 
these  types  of  maneuvers  can  be  applied  to  a  physical  system.  This  process  and  the  results 
are  presented  in  Chapter  VII. 
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II.  DEVELOPING  A  MULTI-BODY  DYNAMIC  MODEL 


A,  THE  NEWTON-EULER  APPROACH 

In  order  to  investigate  the  optimal  performance  of  antenna  slew  maneuvers,  a 
suitable  dynamic  model  must  be  developed.  Due  to  the  complex  and  elaborate  nature  of 
spacecraft,  consisting  of  multiple  hinged  or  gimbaled  joints  fixed  to  lightweight 
structures,  a  multibody  approach  must  be  utilized.  Three  methodologies  were  explored  to 
develop  and  validate  a  multibody  dynamic  model  for  the  spacecraft  antenna  system.  The 
first  is  the  Newton-Euler  approach  described  by  Eric  Stoneking  [15].  This  was  the  main 
approach  used  here  to  generate  a  general  multibody  model  in  MATLAB.  This  model  was 
then  tailored  to  fit  a  variety  of  systems,  including  a  satellite  test  case,  and  a  laboratory 
model,  both  using  TDRS  as  an  operational  baseline.  A  variety  of  other  more  simple 
models  were  developed  to  illustratre  the  process,  and  to  troubleshoot  and  validate  the 
MATLAB  code.  Each  system  was  also  modeled  in  SimMechanics  software  suite  of 
Simulink  in  order  to  perform  additional  verification  and  validation  on  the  MATLAB 
model.  Lastly,  the  SymPy  program,  which  implements  Kane’s  method,  was  also  utilized 
for  verification  and  validation  purposes.  These  latter  two  methods  will  be  discussed  in 
Chapters  III  and  IV.  It  should  be  noted  that  many  software  packages  exist  for  simulating 
a  multi-body  system.  However,  it  is  difficult  to  use  their  output  to  support  optimal  control 
design.  Hence,  a  suitable  model  for  this  purpose  had  to  be  developed  as  part  of  this  thesis. 

The  procedure  outlined  in  [15]  allows  for  the  straightforward  derivation  of  a  very 
complex,  multibody  system  that  can  be  tailored  to  any  jointed- type  spacecraft.  A  general 
procedure  is  described  to  construct  equations  of  motion  in  matrix  form  for  an  N  link 
system,  where  N  is  the  number  of  independently  movable  bodies,  or  links,  in  the  system. 
The  method  is  focused  around  utilizing  a  set  of  state  variables,  organized  as  a  column 
vector.  These  state  variables  can  be  selected  according  to  the  specific  problem  being 
analyzed,  but  it  is  most  convenient  to  utilize  variables  such  as  the  body’s  rotational 
velocity,  O),  or  the  translational  velocity,  v.  It  will  be  shown  that  this  selection  of 
primary  state  variables  will  allow  for  the  easiest  propagation  of  the  model  in  order  to 

solve  for  the  dynamics  of  the  system.  Other  state  variables  are  utilized  and  will  be 
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discussed  further  as  the  approaeh  is  detailed.  These  state  variables  lead  direetly  do  a 
seleetion  of  dynamies  variables.  For  example,  co  io  (b  ,  and  v  to  v .  It  is  these  dynamies 
variables  that  will  be  solved  for  using  this  proeess.  The  prineiple  behind  seleeting  the 
dynamies  variables  is  that  eaeh  one  ean  be  utilized  to  solve  for  other  desired  states  by 
integrating  a  set  of  differential  equations.  For  example,  6)  eould  be  used  to  ealeulate 
rotational  veloeity,  whieh  eould  then  be  used  to  ealeulate  rotational  position.  Onee 
determined,  the  dynamies  variables  (populating  a  state  eolumn  veetor)  are  left  multiplied 
by  a  mass  matrix  and  set  equal  to  a  set  of  eonstraint  terms  in  the  solution  veetor. 
Together,  the  state  veetor,  mass  matrix,  and  eonstraint  veetor  make  up  the  equations  of 
motion  of  the  system  for  translation  and  attitude. 

In  [15],  Stoneking  outlines  proeedures  for  handling  both  spherieal  joints,  and 
gimbaled  joints.  The  proeesses  are  similar,  but  utilize  different  details  for  ineluding  joint 
or  gimbal  eonstraints,  and  for  redueing  the  joint  forees  in  the  dynamies  equations.  One 
key  aspeet  is  that  as  joints  and  links  are  added  to  the  system,  the  eomplexity  rapidly 
grows.  The  spherieal  joint  system  results  in  9N-3  equations,  whereas  the  gimbaled 
system  results  in  6N  equations.  This  is  beeause  the  gimbaled  system  already  ineludes 
some  reduetion  of  redundant  terms  simply  by  virtue  of  its  eonstruetion.  One  of  the  key 
features  of  this  proeedure  is  the  ability  to  build  the  system  through  inspeetion.  To  enable 
this,  the  mass  matrix  and  eonstraint  veetors  are  partitioned  into  smaller  submatriees  and 
subveetors.  Eaeh  submatrix  and  subveetor  eolleets  terms  that  serve  similar  funetions  and 
are  struetured  in  sueh  a  way  that  eaeh  ean  be  eonstrueted  simply  with  knowledge  of  how 
the  system  links  and  joints  are  eonfigured.  The  proeess  is  designed  in  sueh  a  way  that 
generalized  equations  ean  be  used  to  fill  out  eaeh  element  of  these  submatriees  and 
subveetors. 

Furthermore,  row  operations  are  utilized  to  reduee  the  dimensionality  of  the 
system  to  3N  +  3  equations  for  both  the  spherieal  and  gimbaled  system.  The  mass  matrix 
and  eonstraint  veetor  are  restruetured,  and  the  state  veetor  partitioned  into  subveetors  as 
well.  These  new  matriees  and  veetors  are  then  manipulated  to  eliminate  the  equations 
involving  redundant  eonstraint  forees  and  torques  at  the  joints. 
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Overall,  the  method  allows  for  straightforward  eonstruction  of  the  equations  of 
motion  of  a  eomplex  system,  and  provides  a  way  to  reduee  the  dimensionality  and  solve 
for  the  state  variables  of  the  system.  This  is  all  possible  without  the  need  to  perform  a 
lengthy  derivation  of  individual  equations  of  motion.  While  the  following  seetion 
deseribes  the  derivation  of  the  equations  of  motion,  it  is  meant  as  an  aid  to  deseribe  how 
the  matriees  are  eonstrueted  and  solved,  but  full  derivation  is  not  neeessary  to  exeeute  the 
method  itself. 

B,  DERIVING  THE  EQUATIONS  OF  MOTION 

While  a  system  ean  be  deseribed  by  inspeetion  using  the  generalized  equations 
provided  in  [15],  the  equations  of  motion  will  be  developed  in  full  here  to  help  explain 
the  proeedure.  A  generie  three-link  model  was  developed  for  use  in  both  the  test  ease  and 
the  laboratory  model.  This  model  was  then  restruetured  or  truneated  as  necessary  to  suit 
various  system  configurations  under  investigation. 

The  first  step  is  to  define  the  system  configuration  and  reference  frames.  The  first 
link  is  considered  the  base  of  the  system,  while  the  subsequent  links  are  connected  by 
gimbaled  joints  as  shown  in  Figure  5.  An  inertial  reference  frame  is  declared  outside  the 
model,  while  individual  local  reference  frames  are  placed  at  the  center  of  mass  (CoM)  of 
each  link.  These  reference  frames  are  aligned  to  each  link  in  the  manner  most  convenient 
for  the  problem.  For  example,  it  may  be  convenient  to  align  the  frames  to  the  principle 
axes  of  inertia  for  each  link,  or  it  may  be  convenient  to  align  frames  to  match  gimbal 
structure  or  frames  for  spacecraft  sensors. 
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Joint  1 


/  Inertial 
^  Frame 

Figure  5.  Link  Frames  for  Generic  Three-Body  System 

Once  the  frames  are  established,  forces  and  torques  can  be  assigned  to  each  body 
and  gimbal.  External  forces  and  external  torques  are  applied  at  the 

center  of  mass  of  each  body,  as  shown  in  Figure  6. 


Ts 


Figure  6.  Link  External  Forces  and  Torques 


Each  external  force  and  torque  is  assumed  positive  in  the  respective  body  frames. 

Furthermore,  joint  torques  and  joint  forces  {Fg^,Fg^)  are  assigned  to  each 
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gimbaled  joint.  The  torques  and  forces  act  equal  in  magnitude  and  opposite  in  sign  on 
each  body  attached  to  that  joint,  as  shown  in  Figure  7  through  Figure  9.  In  order  to 
generate  the  equations  of  motion,  it  is  assumed  that  these  forces  act  positive  in  the  link 
frame  farthest  from  the  base  (outer  link)  and  act  negative  on  the  link  frame  closest  to  the 
base  (inner  link).  For  example  in  Figure  7,  Fgi  and  Tgi  act  in  the  negative  link  one 
direction  since  it  is  the  innermost  link.  However,  in  Figure  8,  Fgi  and  Tgi  act  in  the 
positive  link  two  direction,  since  it  is  the  outermost  link.  Notice  also  that  the  forces  and 
torques  are  equal  and  opposite  in  magnitude.  This  sign  orientation  will  also  become 
evident  when  the  equations  of  motion  are  constructed. 
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Each  of  these  forces  and  torques  is  a  3x1  column  vector  of  components  acting 
along  the  x,  y,  and  z  axes.  Frames  of  reference  for  forces,  torques,  and  other  variables  will 
be  fully  explored  later,  but  this  stage  of  the  derivation  does  require  assumptions  to  be 
made  for  these  basic  directions.  It  should  also  be  noted  that  some  different  direction 
assumptions  are  used  in  [15].  For  example,  [15]  assumes  the  gimbal  forces  and  torques  to 
be  positive  on  the  inner  link  and  negative  on  the  outer  link.  While  the  selection  is 
arbitrary  at  this  point,  the  choice  must  be  kept  track  of  as  it  affects  how  the  equations  of 
motion  are  written  and  does  result  in  final  equations  of  motion  expressed  in  a  slightly 
different  from  those  given  in  [15]. 

In  order  to  generate  the  equations  of  motion  of  the  system,  each  link  is  inspected 
individually  using  external  and  joint  forces  and  torques.  Two  equations  of  motion  are 
developed  for  each  link,  a  translation  equation  and  a  rotation  equation.  Translational 
motion  derivation  begins  with  Newton’s  second  law,  where  m  is  the  scalar  mass  of  the 
link,  V  is  the  local  time  rate  of  change  of  velocity  of  the  link  as  a  3x1  column  vector, 
and  F  is  the  sum  of  forces  exerted  on  the  link,  also  as  a  3x1  column  vector: 
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mv  =  F 


(2.1) 


Summing  the  forces  for  each  body  results  in  the  following  equations: 


m,v,=F,-F^, 

(2.2) 

mjVi  =  +  Fg\  ~  Fg2 

(2.3) 

—  F^  +^G2  • 

(2.4) 

Rotation  equations  of  motion  can  be  similarly  implemented  by  starting  with 
Euler’s  equation,  where  /  is  the  inertia  tensor  of  the  link  (as  a  3x3  matrix),  co  and  <x)  are 
the  local  angular  velocity  and  time  rate  of  change  of  angular  velocity  of  the  link  as  3x1 
column  vectors,  T  is  the  sum  of  torques  exerted  on  the  link  as  a  3x1  column  vector,  and 
I  CO  is  the  angular  momentum  of  the  link  as  a  3x1  column  vector  [16]. 


l(b  =  T  -  cox  Ico  (2.5) 

It  is  important  to  note  that  Equation  (2.5)  assumes  there  is  no  momentum  storage 
in  the  system,  such  as  reaction  wheels.  The  torque  term  in  Equation  (2.5)  can  be  further 
decomposed  into  three  different  torques:  external  torque,  joint  torque,  and  the  moment 
exerted  by  forces  on  each  link  due  to  the  joint.  Assuming  the  external  forces 

are  applied  at  the  center  of  mass  of  the  link,  there  are  no  couples  generated  by  these 
forces.  However,  each  link  is  still  subject  to  the  moments  created  by  the  joint  constraint 
forces  (E’gi,Fg2).  Computing  these  torques  requires  the  addition  of  a  moment  arm 

variable,  r  .  There  is  a  moment  arm  for  each  joint  of  each  link.  Eink  one  and  three  each 
connect  to  one  gimbal,  while  link  two  connects  to  two  gimbals,  resulting  in  a  total  of  four 
moment  arms.  The  moment  arms  are  measured  from  the  center  of  mass  of  each  link  to  the 
respective  gimbal  and  are  measured  in  the  local  link  frame.  Each  variable  is  denoted  by  a 
two  number  subscript.  The  first  number  in  the  subscript  is  the  link  and  the  second  number 
is  the  gimbal.  Eor  example,  the  moment  arm  for  link  two  to  gimbal  one  is  .  This  results 

in  the  four  moment  arms  as  a  3x1  column  vector,  as  shown  in  Eigure 

10. 
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/  Inertial 
^  Frame 

Figure  10.  Link  Moment  Arms 

With  these  variables  in  place,  the  rotation  equations  of  motion  can  be  generated. 

(2-6) 

1 2^1  ~  2^2  +  Fgj  —  Tg2  +  (^21  ^  ^G\  )  ~  (^22  ^  ^G2  )  ~  {^2  ^  ^2^2  )  (2-7) 

~  2^2  (^2  ^ ^02)  ~  (®3  ^ ^3^3}  (2-8) 

Notice  the  sign  change  on  the  joint  torques  between  the  equations  of  motion  for 
each  body.  It  is  also  important  to  note  that  each  moment  arm  is  established  in  the  local 
link  frame;  this  can  result  in  some  confusion  when  using  these  terms  in  conjunction  with 
gimbal  forces  to  generate  moments.  Notice  also  the  duplication  of  joint  forces  and  joint 
torques  in  Equation  (2.6)  through  Equation  (2.8).  Each  joint  force  and  torque  is  applied 
equal  and  opposite  on  each  link,  as  discussed  previously.  Flowever,  in  order  to  properly 
orient  each  force  and  torque  to  each  link  as  the  system  moves,  coordinate  transformations 
must  be  applied.  These  coordinate  transformations  will  be  necessary  for  a  variety  of 
components,  not  just  forces  and  torques.  However,  for  this  stage  of  the  derivation,  it  is 
easiest  to  leave  the  equations  in  a  generic  reference  frame.  The  equations  of  motion  will 
be  derived  fully  in  accordance  with  the  procedure  in  [15],  and  then  proper  coordinate 
transformations  will  be  applied  where  necessary.  This  delay  in  applying  coordinate 
transformations  makes  it  doubly  important  that  proper  attention  is  given  to  tracking 

reference  frame  orientations  and  sign  assumptions  throughout  the  derivation. 
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For  example,  suppose  link  two  is  a  simple  rod,  one  meter  long  with  a  gimbal  at 
each  end.  The  x-axis  of  link  two  is  aligned  to  the  length  of  the  rod,  positive  toward 
gimbal  two,  with  a  center  of  mass  at  0.5  meters  along  the  length.  This  results  in  a 
Tjj  of  [-0.5;  0;  0]  meters  since  gimbal  one  is  in  the  negative  direction  from  the  center  of 

mass.  Alternatively,  r22  is  [0.5;  0;  0]  meters  since  gimbal  two  is  in  the  positive  direction 
from  the  center  of  mass.  The  moment  arm  generates  a  moment  when  crossed  with 
Fgj .  Since  is  assumed  positive,  the  torque  generated  by  this  cross  product  is 
assumed  positive,  .  However,  in  reality,  the  generated  torque  would  be  negative 

in  the  link  frame  due  to  the  negative  first  element  value  of  .  Conversely,  {r^2 
assumed  negative  due  to  being  negative  in  the  link  2  frame.  Since  the  elements  of  r22 
are  positive,  this  results  in  a  negative  torque  in  the  link  frame.  Errors  in  the  assumptions 
on  the  signs  of  the  forces  will  become  apparent  when  the  system  is  simulated. 


Figure  1 1 .  Example  Layout  of  Link  Moment  Arms  (Link  2) 


Throughout  the  derivation  of  equations  of  motion,  many  of  these  types  of 
arbitrary  assumptions  must  be  made  regarding  directions  and  frames.  While  the  decisions 
can  be  made  to  accommodate  specific  problems  or  setups,  they  will  not  change  the 
functionality  of  the  final  equations  of  motion  as  long  as  they  are  properly  tracked  and 
used  uniformly  throughout  the  process.  As  mentioned  above,  any  errors  in  the  assumed 
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signs  of  the  internal  forees  and  moments  will  be  obvious  when  the  equations  of  motion 
are  solved. 

While  Equations  (2.2)  through  (2.4)  and  (2.6)  through  (2.8)  are  suitable  for  eaeh 
link  individually,  they  do  not  deseribe  the  system  as  a  whole.  To  consider  these  links  as  a 
series  of  connected  bodies,  additional  constraint  equations  are  required.  The  translational 
velocities  of  the  gimbals  as  points  in  space  (not  the  internal  angular  rates  of  the  gimbals) 
are  solved  for  both  links  connected  to  that  gimbal  and  set  equal  to  one  another.  In  other 
words,  the  velocity  of  the  coincident  point  on  two  different  links  is  the  same  since  the 
links  are  joined  at  that  point.  To  compare  the  two  values,  however,  both  velocities  must 
be  solved  and  set  in  the  same  frame.  First,  the  velocities  of  each  gimbal  are  calculated 
using  the  velocity  of  the  center  of  mass,  v ,  the  rotational  velocities  of  the  link,  <»,  and  the 
position  of  the  gimbal,  r  [15]. 


=H+(®lX'il) 

(2.9) 

VG.=r+{(^2^r) 

(2.10) 

Vo. 

=  Vj  +  ((»j  X  Tjj )  =  Vj  +  (ryj  X  ^21 ) 

(2.11) 

Vg2  ~'^2  "*"  (  ^2  X  ^22  ) 

(2.12) 

Vq2  —  ^2  +  (  ^2  X  ^32  ) 

(2.13) 

Vo2 

—  Vj  +  (<^2  X  ^22  )  ~  ^3  X  ^22  ) 

(2.14) 

The  time  derivative  of  Equations  (2.11)  and  (2.14)  are  then  taken.  However,  they 
must  be  compared  in  the  same  frame;  the  inertial  frame  will  be  assumed  for  this 
derivation.  This  leads  to  the  generation  of  some  additional  terms  due  to  centripetal 
acceleration  as  seen  in  Equation  (2.15)  [17]. 

—  {x)=  — (x)+^(y‘xx  (2.15) 

dr  ’  dt 

Equation  (2.15)  and  the  chain  rule  can  be  used  to  transform  Equation  (2.11)  and 
Equation  (2.14)  into  equations: 
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(2.16) 


Vi  +  (^>1  X  Til  )  +  ((<>1  X  /Jj  )  +  [o),  X  X  Til  ))  =  ... 
...V2  +{(i>2  X^2l)  +  ('^2  ^4i)  +  ('^2  >^{(^2  ^^21)) 

Vj  +  ((^2  X  ^22)  +  (<^2  X  ^2)  +  (<^2  X  {0)2  X  ^22))  =  ... 

...V3  +  ((^3  X r32)  +  ((<>3  X  4)  +  ((^3  X ((<>3  X r32)) 


However,  since  the  r.j  =  0  for  non-prismatic  joints,  these  terms  can  be  removed. 


fi  +  (4  X  rji )  +  ((^1  X  ((^1  X  rji ))  =  4  +  (®2  X  r2i )  +  ((^2  X  ((^2  X  ))  (2. 1 8) 

4  +  (^2  X  r22)  +  ((^2  X  ((^2  X  r22))  =  63+  {(b,  X  +  ((^3  x{ct>,  X  r32))  (2.19) 


At  this  point,  it  is  convenient  to  introduce  some  notation  to  simplify  expression  of 
the  cross  products  and  double  cross  products.  Each  vector  cross  product  can  be 
represented  by  transforming  the  left  vector  into  a  3x3  skew  symmetric  matrix  described 
in  Equations  (2.20)  and  (2.21)  [15].  The  matrices  are  constructed  such  that  when  the 
matrix  is  left  multiplied  by  a  vector,  it  is  equivalent  to  the  cross  product  (or  double  cross 
product)  of  the  two  vectors. 


X  = 


X  = 


x^x, 

x^x^ 


2  2 
-X3  -Xj 

X3X2 


X2X3 

2  2 
-Xj  -X2 


(2.20) 


(2.21) 


Eurthermore,  in  order  set  up  the  equations  for  future  steps,  the  order  of  the  cross 
products  is  reversed,  which  changes  the  sign  of  each  cross  product  term.  This  simplifies 
Equations  (2.16)  and  (2.17)  into  Equations  (2.22)  and  (2.23). 


Vj  +  <^^11  —  ^2  ^21®2  ^^21 


(2.22) 


^2  ^22  ®2  ®2^22  ^3  ^32  ®3  ®3^32 


(2.23) 


Using  Equations  (2.2)  through  (2.4);  Equations  (2.6)  through  (2.8);  and 
Equations  (2.22)  and  (2.23),  the  dynamics  of  a  three-link  system  can  be  described  using  a 
set  of  ordinary  differential  equations  based  on  the  variables, (hip  <i’2’‘^3AiA2A3’FGi’pG2) 
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.  External  torques,  external  forces,  joint  torques  and  system  configuration  variables  such 
as  inertia  and  moment  arms  are  considered  inputs  into  the  problem.  Additional  states, 
including  rotational  velocity (tUj, rU2 ’ ^*3)  >  rotational  position  (S^,S2,S^),  translational 
velocity  (Vj,V2,V3),  and  translational  position  (dj,d2,d3),  ay  be  obtained  by  integrating 
the  base  state  variables. 

C.  TAILORING  TO  GIMBALED  JOINTS 

The  equations  of  motion  described  in  Section  B  can  be  further  tailored  to  the 
desired  gimbaled  system  by  utilizing  the  gimbal  angle  rates  to  determine  link  angular 
rates.  First,  two  new  variables  are  introduced,  9  and  E .  Each  gimbal  has  a  column 
vector,  9  ,  which  describes  the  Euler  angles  for  the  gimbaled  joint.  These  angles  are  used 
to  build  kinematic  differential  equations,  codified  in  the  E  matrix,  which  relates  gimbal 

rates,  9 ,  to  link  angular  rotation  rates,  co,  as  shown  in  Equation  (2.24)  and  Equation 
(2.25)  [18]. 

C02  =  C0\+  Ej^j  (2.24) 

CO^  =  <^>2  ^2^2  ~  ^2^2  (2-25) 

Each  E  matrix  will  utilize  a  specific  Euler  sequence  that  the  gimbal  rotates 
through.  The  Euler  sequences  can  be  described  as  either  body-fixed,  or  space-fixed  and 
can  use  any  number  of  sequential  rotations  (see  [18]  for  details).  For  example,  a  body- 
three  x-y-z  sequence  utilizes  three  successive  rotations,  each  about  the  body  axes.  First, 
the  body  is  rotated  about  its  x-axis  by  9^ .  Next  it  is  rotated  about  its  y-axis  by  9^, .  Eastly 

it  is  rotated  about  the  body  z-axis  by  9^ .  Similarly,  a  body-three  y-x-z  rotation  would 
rotate  about  the  y-axis  by  9^ ,  then  about  the  x-axis  by  9^ ,  then  about  the  z-axis  by  9^ . 

Rotations  could  also  be  made  about  a  non-body  fixed  axis,  the  inertial  axes  for  example. 
This  is  referred  to  as  a  space  sequence.  For  example,  a  space-three  x-y-z  using  the  inertial 
frame  would  rotate  the  body  around  the  inertial  x-axis  by  9^ ,  then  the  inertial  y-axis  by 

9y ,  and  finally  the  inertial  z-axis  by  9^ .  Which  sequence  is  utilized,  the  number  of 

rotations,  and  whether  or  not  to  use  body  or  space  sequences,  is  dependent  on  the 
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problem  being  solved.  In  this  thesis,  body-three  x-y-z  rotations  were  implemented  for  the 
gimbaled  system.  This  ehoiee  results  in  the  F  matrix  in  Equation  (2.26)  [18]. 


r  = 


cos  cos 
-COS0.  sin0 
sin 


sin^^ 

cos^^ 

0 


0 

0 

1 


(2.26) 


A  variety  of  kimenatic  differential  equations  based  on  body  and  space  rotations 
are  provided  in  [18].  However,  their  chosen  notation  is  different  from  what  is  used  here 
and  is  worth  clarifying.  An  x-y-z  rotation  is  assumed,  either  body  fixed  or  space  fixed. 
Here,  subscripts  for  the  angles  are  maintained  as  x,  y,  and  z  components.  Therefore,  the 
sequence  would  first  rotate  about  the  x-axis  by  ,  then  the  y-axis  by  0^ ,  then  the  z-axis 

by  .  However,  most  literature,  including  [18]  does  not  utilize  the  x,y,z  notation;  instead 
they  use  a  1,2,3  notation.  However,  the  angles  given  in  [18]  are  listed  in 

sequential  order  of  operations,  not  according  to  their  relative  axis.  For  example,  for  a  1- 
2-3  rotation,  the  sequence  would  rotate  about  the  first  axis  by  0^ ,  then  the  second  axis  by 
0^,  then  the  third  axis  by  0^ .  However,  for  a  2-1-3  rotation,  the  sequence  would  rotate 
about  the  second  axis  by  6^ ,  then  the  first  axis  by  ,  then  the  third  by  6^ .  This 

difference  in  notation  can  lead  to  confusion,  so  it  is  important  that  any  assumptions  are 
properly  carried  through  the  entire  derivation. 

Equation  (2.24)  and  Equation  (2.25)  are  differentiated  using  the  chain  rule  to 
derive  Equations  (2.27)  and  Equation  (2.28),  which  give  the  angular  accelerations.  These 
derivatives  generate  additional  terms  according  to  Equation  (2.15).  It  is  important  to  note 
that  the  additional  cross  product  seems  to  have  been  incorrectly  omitted  in  [15]. 

<^>2  =  <h|  +  f  -l-  Ej^j  -l-  (O2  X  Ej^  (2.27) 

^3  =  ^2+  +  ^3  X  T2O2  =  ... 

.....  .  (2.28) 
...6)^  +  Ej^j  -|-  Ej^j  -|-  COj  X  Ej^  -|-  r2^2  ^2^2  ^  i^2^2 
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The  equations  can  be  further  expanded  by  inserting  the  rates  for  link  two  and 
three  (Equation  (2.24)  and  Equation  (2.25))  and  the  time  rate  of  change  of  angular 
velocity  of  link  two  (Equation  (2.27)). 


<^>2  —  +  f  +  Ej^j  +  (<Uj  +  Ej^)  X  Ej^  —  +  Ej^j  +  E^^j  +  co^  ^0  (2.29) 


d)^  —  rhi  +  Ej^j  +  +  rUjEj^j  +  r2^2  ^2^2  ^  (®i  ^  ^  ^2^2)  ^  ^2^2  ~  ••• 

0)^  +  f  +  Ej^j  +  f  2^2  ^2^2  ^  ^1(^1^!  +  E2^2)  (^1^1  ^  ^2^2) 


(2.30) 


Inserting  these  equations  back  into  Equations  (2.22)  and  (2.23)  yields 
Equations  (2.31)  and  (2.32). 


Vj  -  TiitUi  +  =  V2  -  r2i 


+  Ej^j  +  Ej^j  +  ujj  X  Ej^^  +  ^>2 


(2.31) 


I  +  0)2^22  —  ... 


V2  -  ^22  +  f  +  Ej^j  +  (Z)j  X  Ej^^  - 

Vj  —  r22((yj  +  f  +  Ej^j  +  f  2^2  ^  ^2^2  ^2^2)  (^1^1  ^  ^2^2))  ^  ®?^2  ^2  32) 

Rearranging  the  terms,  equations  for  the  time  rate  of  change  of  translational 
velocities  of  link  two  and  three  can  be  derived. 

V2  —  Vj  +  (i^j  —  +  ^iE[^]  +  ^2^ ^0^  +  i^jUijEj^  +  —  <^>2^21  ^2  33) 

^3  “  ^2  (^32  ~  ^2)®!  (^2^1  ~  ^2^l)^l  ^32r2^2"- 

...-4®iEj^i  +4f  1^1  +41^24  +4®i(ri^i  +E24)-  (2.34) 

...  +  i"32(Ej^[  X  E2^2)  ®2^22  ~  ®3^32 

The  equation  for  link  three  can  be  further  expanded  by  inserting  the  time  rate  of 
change  of  velocity  of  link  two. 

V3  —  Vi  +  (r32  “^22“*"  ^21  ~  hl)®l  (^2^1  ~  ^2^1  ^1^1  )^1  ^2^2^2  ”*“••• 

...(^2®1  ^1®1  ~  ^2®l)^l^  (^2  ~  ^22  ^32^2^2  •"  (2.35) 

...f32(»j(E2^2)  +  ^32(^1^!  X  E2^2)  +  ®2^22  “  ®3^32  +  ®lhl  “  ®2^21 

These  equations  can  now  be  inserted  into  the  equations  of  motion  for  translation 
and  rotation  for  link  two  and  three  (link  one  Equations  (2.2)  and  (2.6)  are  unaffected). 


m2Vy  +^2(^21  +^2^21^1^!  +^2^21^16*1  +m2r2j«jEj^  +  m2(X>irjj  -m2C02h\  ~  ••• 


F  +F  -F 

...1  2^  ^  G\  ^  G2 


(2.36) 
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Tn^V^+ Tn^{r^2  ^11)^1  ^3(^32^!  ^2^1  '*'^1^1  )^1  '*'^3^32r2^2  '*'••• 

...m3(f32^1  +  fjjOjj  -  +  ^3(^32  ~  ^22  +  ^2l)f  1^1  +  fn3^32^2^2  +•" 

...m3r32(«jr2^2)  +  ^3^32(ri^l  X  r2^2  )  +  ^3  (^2^22  “  ^3^32  +  ^1^11  “  ^2^2l)  =  ••• 

F  +F 

...J  3  T  J  g2 


(2.37) 


/2<y[  +  12^1^1  + 


^2  '*' ^G1  ^G2  '*' ^21-^0 1  ^22^G2  ^2{^2^2^  (2.38) 


+/3rj(9j  +  /3f24  +  /3r24  +  A(®ir24)+/3(rj^j  XT2O2)  -  ■■■  39) 

...T3  +  Tq2  +  ^32^02  ~  ^3  (A^b) 

Equations  (2.2),  (2.6),  and  (2.36)  through  (2.39)  fully  describe  the  system  using 
the  translational  and  angular  acceleration  for  link  one,  the  gimbal  acceleration,  and  the 

gimbal  constraint  forces  (^<hj,^j,^2’’^i’FGi’FG2)  •  External  forces;  external  torques;  gimbal 

torques;  position  and  attitude  velocities  of  link  two  and  three;  and  gimbal  positions  and 
rates  are  all  considered  inputs  or  known  at  each  instant  in  time.  In  order  to  solve  this 
system  of  six  equations,  a  matrix  based  approach  is  outlined  in  [15],  which  is  described 
next  in  Chapter  III. 
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III.  SOLVING  THE  EQUATIONS  OF  MOTION 


A,  IMPLEMENTING  EQUATIONS  OF  MOTION  IN  MATRIX  FORM 


First,  the  six  equations  of  motion  developed  in  Chapter  II  are  rearranged  to  place 
the  terms  that  include  the  dynamics  variables  and  joint  forces  {(b^,0y,02,'v^,VQ^,VQ^)  on 
one  side,  and  the  terms  that  include  the  inputs  or  knowns  on  the  other. 


Vl+^Gl  =^1 

=  - 

A  -  A  -  +  ^2 


(3.1) 

(3.2) 


+"^3(4  -  hi  +  4  -  4)®1  +"^3(44+44  -r22TAe,+m/22^2^2- 

...—  F  Q2~-  — ^3(h4l  —  4^1  ^2^1  )^1  —  ^3(^32®!  +  Cl4  —  h24)^l^r" 

2^2  ~  ^2^22^^A  2^2^  ~  ^2^22^^  A\  2^2^'" 

...  —  —  (02^2\  ^2^22  ~  ^2^22)  4 


/j<hj  +  rjjFjjj  7J  Tgj  <hj(/jrUj) 


44  +  ^2^ Ax  -  Ci4i + 442  =  4  +  Tax  -  Tg2  -  4  (4®2 )  -  4ri4  -  444^  (3-5) 


44  4^14  4^2^  ^22^G2 

4  +  4;2  -  4  (4^3)  +  4^14  +  4^24  -  4(444)  -  4(^)4  r24) 


(3.6) 


Note,  that  while  there  are  some  time-rate-of-change  components  on  the  right  hand 
side,  these  do  not  include  the  defined  dynamics  variables  and  will  be  solved  for  at  each 
instant  in  time  by  integrating  the  equations  of  motion.  The  dynamics  variables  are  then 
pulled  out  and  placed  into  a  dynamics  variable  column  vector.  The  remaining  terms  on 
the  left  compose  a  mass  matrix,  while  the  terms  to  the  right  side  of  the  equation  are 
appropriately  structured  as  a  column  vector  of  constraint  terms. 
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A 

0 

0 

0 

^11 

0 

h 

Ari 

0 

0 

-^21 

^22 

h 

/3ri 

I2T2 

0 

(? 

~^32 

< 

^2 

S 

0 

0 

0 

»lj 

7 

0 

^1 

^2(4 -^11) 

^2(^21^!) 

0 

ffi2 

-7 

1 

^01 

^3(^21 -^11 +  4 -4) 

^3(^1^!  ^32^1  ~  ^2^1) 

m/22^2 

m3 

(? 

-1 

.^G2. 

Tl  +  Tci  -  T^2  -  ^2  (^2^2  )  -  hi^A)  -  1 2^1^ A 

7^3  +  Tg2  ~  (^2  A'i^A  "^3(^2^2)  ~  "^3(^1^2^2)  ~  "^3(^1^!  ^  ^2^2) 


F, 

-1712^2^ lO^  -  m2r2iS)^^0  -  ^2^/11  +  ^2^2^21  +  ^^2 

-m3(r2ifi  -4fj  +4f  1)^1  -^3(4^),  +r2i^j  -4^1  )r,^i  -^341^2^  -^34(^1^24)  +  - 

“^3^32  (^1^1  ^  ^2^2)  “  ^sC^l^ll  “  ^2^21  +  ^2^22  “  ^3^32)  +  ^^3 


(3.7) 
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Each  term  in  the  mass  matrix  of  Equation  (3.7)  is  itself  a  3x3  matrix.  The  mass 
terms  are  effectively  the  three  mass  scalars  of  each  link  multiplied  by  a  3x3 

identity  matrix  to  yield  a  3x3  mass  matrix.  Similarly,  the  1,  -1,  and  0  terms  are  all  scalar 
values  multiplied  by  a  3x3  identity  matrix.  This  yields  an  18x18  matrix.  Each  variable 

((h,,6^j,6^2’^p^Gi’^G2)  consists  of  a  3x1  column  vector,  resulting  in  a  total  state  vector 

of  18x1  terms.  Eikewise,  each  element  in  the  rightmost  column  vector  represents  a  3x1 
vector,  resulting  in  a  total  constraint  vector  of  18x1  terms.  This  matrix  system  represents 
18  dynamics  equations  based  on  18  variables. 

Each  matrix  may  be  further  partitioned  into  sub  matrices.  This  is  useful  for  two 
reasons.  Eirst,  one  of  the  main  goals  is  to  outline  a  process  to  describe  complex  systems 
and  build  their  dynamics  by  inspection.  By  partitioning  the  larger  dynamics  and 
constraints  into  submatrices,  it  is  easier  to  build  each  component  without  first  describing 
the  full  equations  of  motion  for  each  link  as  was  performed  here.  Stoneking  outlines  a 
process  generalized  to  bodies  and  N-\  joints,  and  then  provides  general  equations  to  fill 
out  each  submatrix  by  inspection  [15].  The  mass  matrix  is  partitioned  into  five 
submatrices,  (/,P,n,//,  J)  as  well  as  a  3A6c3  matrix  populated  with  zeros.  The  constraint 
matrix  is  broken  into  two  submatrices,  {T,F).  These  two  sets  of  partitions  will  be 
discussed  in  further  detail  in  Section  C.  The  complexity  of  the  equations  of  motion,  and 
thus  the  submatrices,  grows  rapidly  with  each  additional  gimbal  and  link,  hence  the  need 
for  a  simple,  straightforward  approach  based  on  building  each  submatrix  individually. 
The  three-body,  two-gimbal  system  is  simple  enough  to  describe  in  equation  form  before 
matrix  implementation,  therefore  the  generalized  equations  were  not  utilized  here.  Instead 
the  equations  were  derived  fully,  and  then  translated  into  matrix  form.  The  generalized 
equations  as  well  as  a  derivation  for  a  similar  three-body  system  using  spherical  joints 
instead  of  gimbaled  joints  can  be  found  in  [15]. 

The  second  reason  for  using  submatrices  is  to  reduce  the  state  vector  by 
eliminating  the  joint  equations.  The  process  described  thus  far  has  resulted  in  a  system  of 
6N  equations,  or  18  total  equations  for  a  three  body  system.  However,  it  is  assumed  that 

knowledge  of  the  joint  forces  (Fqi,Fq2)  is  not  required  and  these  forces  can  be 
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incorporated  into  the  overall  problem  by  row  operations.  This  eliminates  the  3N-3  joint 
forees,  resulting  in  a  3A^+3  system.  In  order  to  perform  this  operation,  Stoneking  further 
partitions  the  mass  matrix  into  four  submatriees,  {A,R,S,U) ,  and  the  eonstraint  vector 
into  two  subvectors,  {T',F').  Furthermore,  the  state  veetor  is  partitioned  into  two 
subveetors,  {x,Fq)  .  These  three  partitions  will  be  diseussed  in  further  detail  in 

Section  D.  While  the  matriees  and  veetors  of  Equation  (3.7)  ean  be  eonverted  direetly 
into  these  submatrices  and  subveetors,  it  is  convenient  to  first  partition  the  matriees  and 
veetors  into  the  submatriees.  This  allows  for  an  easier  understanding  of 

what  eaeh  term  in  the  mass  matrix  contributes  to  the  overall  equations  of  motion,  as  well 
as  how  eaeh  term  interaets  with  the  state  variables. 

B,  REFERENCE  FRAMES 

Before  partitioning  ean  begin,  deeisions  need  to  be  made  regarding  coordinate 
frames  of  eaeh  variable  and  of  eaeh  equation  of  motion  as  a  whole.  For  simplieity. 
Equation  (3.7)  was  provided  in  general  form.  No  deelarations  have  been  made  as  to  what 
eoordinate  frame  eaeh  term  is  in,  so  the  equations  are  presented  in  a  purely  general 
nature,  with  no  regard  to  coordinate  frame  de-eonflietion.  In  praetice,  however,  the 
coordinate  frames  of  each  variable  must  be  considered  carefully  and  managed  so  that 
eaeh  equation  of  motion,  eodifred  in  each  row,  utilizes  a  consistent  eoordinate  frame.  The 
selection  of  coordinate  frames  is  ultimately  arbitrary  as  long  as  proper  conversion 
proeedures  are  followed;  a  veetor  expressed  in  frame  A  is  equivalent  to  that  same  veetor 
expressed  in  frame  B.  However,  some  frames  prove  to  be  more  eonvenient  than  others 
and  will  be  seleeted  as  sueh.  To  begin,  it  is  most  eonvenient  to  express  all  forees  as  well 
as  the  translational  veloeity  and  aeceleration  of  link  one  in  the  inertial  frame.  The 
remaining  terms  are  expressed  in  the  local  frame  of  each  link  (see  Table  1).  Gimbal 
torques  are  defined  in  the  outer  link  frame,  as  diseussed  in  Chapter  II. 
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Inertial 

Erame 

Eink  One 
Erame 

Eink  Two 
Erame 

Eink  Three 
Erame 

h 

h 

^21 ’^1’ ^22 ’^22 

^32 ’^32 

Fg\ ’  Fqi 

Ct)2,d>2,Cb2,Cl>2 

T 2^2, 02,62, 62 

T, 

F2’Fqi 

T  T 

Table  1.  Variable  Reference  Frames. 


While  these  frames  were  chosen  because  of  the  convenience,  each  equation  of 
motion  uses  a  number  of  terms  from  each  frame.  Therefore,  the  vectors  must  be  placed  in 
a  common  coordinate  frame  for  each  equation  of  motion,  or  row  of  Equation  (3.7).  Each 
row  can  utilize  a  different  frame,  but  the  frames  must  be  consistent  across  that  row.  To 
ensure  the  frames  match,  direction  cosine  matrices  (DCM)  will  be  used  to  convert 
between  frames.  A  DCM  is  an  orthogonal  3x3  matrix  that  converts  a  vector  from  one 
frame  to  another  utilizing  a  series  of  subsequent  rotations  based  on  relative  angles 
between  the  two  frames.  Eiterature  utilizes  a  vast  array  of  notation  schemes  for  DCMs, 
but  the  notation  that  will  be  used  here  is  “C*  where  C  indicates  a  DCM,  b  is  the  original 
frame  of  the  given  vector,  and  a  is  the  desired  frame  in  which  the  vector  is  to  be 
expressed.  Eor  example,  if  vector  x  is  originally  expressed  in  frame  7>,  indicated  by  ^x, 
and  it  is  desired  to  utilize  vector  x  in  frame  a ,  indicated  by  “x ,  then  the  DCM  would  be 
used  as  follows  [16]; 

“x  =  [''C'’]^.  (3.8) 

The  same  DCM  can  be  utilized  to  transform  from  frame  a  back  to  frame  b 
utilizing  Equation  (3.9),  as  shown  in  Equations  (3.10)  and  (3.11)  [16]. 


ec=i=ce 


b 


b 


X 


X 
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X  = 


(3.9) 

(3.10) 

(3.11) 


Similar  to  the  Euler  sequenees  previously  used  to  eonstruet  the  E  matrix,  the 
rotations  of  a  DCM  ean  be  expressed  as  a  sequenee  of  rotations  in  either  body-fixed  or 
spaee-fixed.  Here,  it  is  easiest  to  use  a  body-fixed  x-y-z  rotation  when  eonverting  baek 
and  forth  between  eaeh  link  due  to  the  nature  of  the  gimbaled  joints  and  the  eonfiguration 
of  the  system.  This  is  based  on  the  assumption  that  the  multiple  axis  gimbals  physieally 
perform  one  rotation  at  a  time,  eaeh  rotation  built  upon  the  last,  using  a  system  of 
subsequent  single  axis  rotations.  This  assumption  would  not  hold  for  a  spherieal  joint,  for 
example  a  ball  joint,  whieh  ean  perform  multiple  rotations  simultaneously.  The  gimbal 
angles  within  the  veetors  9^  and  ^2  will  be  used  to  form  a  DCM  to  eonvert  between 

frames  one  and  two,  and  between  frames  two  and  three,  respeetively.  This  yields  the 
following  DCMs  [18] 

cos  cos  0^^  sin  0^^  sin  0^^  cos  0^^  +  cos  0^^  sin  0^^  -  cos  0^^  sin  0^^,  cos  0^^  +  sin  0^^  sin  0^^ 

=  -cos^jySin^j^  -sin^j^sin^j^,  sin^j^ +cos^j^cos^^  cos^j^sin^j^,  sin^j^ +sin0j^cos^j^ 
sin  0^^,  -  sin  0^^  cos  0^^  cos  6^^  cos  0^^ 

(3.12) 

cos  6*2^  cos  6*2^  sin  6^^  sin  9^^,  cos  9^^  +  cos  9^^  sin  9^^  -  cos  9^^  sin  9^^  cos  9^^  +  sin  9^^  sin  9^^ 

=  -  cos  6*2y  sin  9^^  -  sin  6*2^  sin  9.^^  sin  9^^  +  cos  9^^  cos  9^^  cos  9^^  sin  9.^^.  sin  9^^  +  sin  6*2^  cos  9^^ 

sin  6^22,  -  sin  6^2^  cos  9.^^,  cos  6^2^  cos  9^^ 

(3.13) 

Kane,  Likins,  and  Levinson  provide  DCMs  for  any  eombination  of  body  or  spaee 
eonversions  [18].  However,  eaeh  veetor  in  [18]  is  eonsidered  to  be  a  row  veetor,  whereas 
veetors  are  treated  here  as  column  vectors.  Therefore,  the  DCMs  provided  in  [18]  must  be 
transposed  to  give  the  same  in  the  DCMs  as  Equations  (3.12)  and  (3.13).  Lurthermore, 
the  same  notational  discrepancy  noted  before  in  Chapter  II  for  the  kinematic  differential 
equations  applies  to  the  DCMs  provided  in  [18]. 

While  the  gimbal  angles  are  ideal  for  building  the  transformations  between  each 
link,  the  transformation  between  the  inertial  frame  and  link  one  still  remains.  Since  link 
one  serves  as  the  main  body  of  the  system,  it  will  rotate  about  each  axis  simultaneously 
rather  than  one  rotation  performed  after  another  is  complete.  This  does  not  lend  itself 
easily  to  the  classical  build  described  in  Equations  (3.12)  and  (3.13).  Lurthermore,  in 

order  to  avoid  singularities  that  could  result  from  application  of  a  traditional  Euler  angle 
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(roll,  pitch,  yaw)  approach,  a  set  of  four  quaternion  parameters  (gj,g2,^3,^4)  are  utilized 

instead.  A  thorough  explanation  of  quaternions  will  not  be  given  here.  Instead,  equations 
and  principles  useful  to  the  specifie  system  and  problem  of  interest  in  this  thesis  will  be 
discussed  and  implemented.  A  full  diseussion  of  quaternions  is  presented  in  [16]. 


Unlike  Euler  angles,  quaternions  are  not  independent;  they  are  eonstrained  by 
Equation  (3.14)  [16].  This  additional  constraint  term  is  what  allows  quaternions  to  avoid 
many  of  the  geometric  singularities  that  arise  when  performing  unrestrained  rotations 
using  Euler  angles  [16]. 

+^2  +^2  +qA  (3.14) 

Quaternions  are  derived  from  a  single  rotation  of  the  system  about  an  eigenaxis. 
The  eigenaxis  is  a  vector  that  is  fixed  in  two  frames.  Eor  the  purpose  of  this  system,  the 
frames  are  the  body  frame  of  link  one  and  the  inertial  reference  frame.  This  is  represented 
in  Equation  (3.15),  where  e  represents  the  eigenaxis  and  subsequent  components,  L 
represents  the  eomponents  of  the  link  one  frame,  and  N  the  eomponents  of  the  inertial 
frame  [16]. 

e  =  gjLj  +  (3.15) 


The  quaternions  are  defined  in  Equation  (3.16),  where  e^,  e^,  and  63,  are  the 
vector  components  of  the  eigenaxis  of  rotation,  and  6  is  the  angle  by  which  the  system  is 
rotated  about  that  axis  [16]. 


^2 

^3 


■ 

e,  sin— 
2 

.  6 

e,  sin— 
'  2 

.  e 

e.sm— 
'  2 

e 


cos— 

2  J 


(3.16) 


Consider  a  body  frame,  L  ,  initially  coincident  with  the  inertial  frame,  N ,  then 
rotated  about  only  the  x-axis.  Since  the  x-axis  of  the  body  and  the  x-axis  of  the  inertial 
frame  remain  eoineident  through  the  rotation,  the  eigenaxis  is  [1,0,0]^.  This  results  in  the 
four  quaternions; 
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=  <^ 


(3.17) 


q.  = 


q.i 

.^x4. 


•  0. 
Sin  — 
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cos- 


The  same  process  could  be  made  for  individual  rotations  about  the  y  and  z  axes. 
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(lyX 

^y2 

qy. 


e„ 


sm- 


e,. 


cos- 


(3.18) 


q.  = 


0 

q^ 

0 

q.2 

.  e 

—  ^ 

sin  — 

q.2 

2 

q.A^ 

cos  — 

1  2  J 

(3.19) 


Furthermore,  successive  rotations  of  quaternions  can  be  determined  using  the 
quaternion  multiplication  as  shown  in  Equation  (3.20),  where  q  is  the  first  rotation  and 
q'  is  the  second  rotation,  resulting  in  a  total  rotation  described  by  q"  [16]; 


q'A 

q'2 

-q'2 

q'x 

qx 

-q'2 

q'A 

q'x 

q'2 

q2 

< 

q'2 

-q'x 

q'A 

q'2 

q2 

_-q'i 

-q'2 

-q'2 

q'A_ 

(3.20) 


Equations  (3.17),  (3.18),  and  (3.19)  can  be  inserted  one  at  a  time  into 
Equation  (3.20)  to  yield  the  total  x-y-z  rotation.  Eirst,  the  rotations  about  the  x  and  y  axes 
are  used  to  find  the  quaternions  for  an  x-y  rotation. 
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Then  the  x-y  quaternions 
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(3.21) 
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(3.22) 


Conversely,  quaternions  can  be  used  to  determine  the  components  of  a  DCM,  as 
shown  in  Equation  (3.23)  [16]. 


l-2{q,^  +  q,^) 


C  = 


liqiq^-q.qj 

liq.q^+qiqj 


liq^q^+q.q,) 

\-2(q,^+q,^) 

2{q,q^-q^qJ 


2(^i^3-^2^4) 
2{q^q^+q,q^) 
\-2iq,^  +  q,^) 


(3.23) 


Lastly,  the  time  rate  of  change  of  the  quaternion  vector  can  be  solved  for  using  the 
current  quaternions  and  the  body  angular  velocity  as  shown  in  Equation  (3.24)  [16].  This 
will  be  used  later  to  solve  for  the  time  history  of  the  quaternion  vector. 
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(3.24) 


Overall,  Equation  (3.12),  Equation  (3.13),  and  Equation  (3.23)  yield  three  DCMs 


to  transform  back  and  forth  between  the  inertial  frame  and  the  body  frames  of  link  one, 
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two  and  three  'C^,  ^C\  .  These  DCMs  ean  be  transposed  to  yield  DCMs  to  eonvert  in 

the  other  direetion,  or  staeked  to  yield  DCMs  between  non-eonseeutive  frames  as 
follows; 


=[‘C^  J 

(3.25) 

‘c'  =  ['c‘]^ 

(3.26) 

(3.27) 

_  2^1 

(3.28) 

\^N  _  3^2  2^1 

(3.29) 

3^1  ^  3^2  2^1^ 

(3.30) 

With  these  DCMs  defined,  the  dynamies  and  eonstraint  matriees  ean  be  easily 
deeomposed  into  submatriees  and  eonverted  to  eonvenient  eommon  frames. 

C.  SUBMATRIX  PARTITIONING 

To  further  east  the  equations  of  motion  into  a  form  suitable  for  numerieal 
simulation,  the  mass  matrix  is  first  broken  into  the  five  submatriees  and  a  zero  matrix. 
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(3.31) 

The  /  matrix  relates  the  inertia  of  eaeh  link  to  the  attitude  dynamies  of  that  link. 
The  first  eolumn  is  multiplied  by  the  time  rate  of  ehange  of  angular  veloeity  and  denotes 
how  that  link  is  affeeted  by  the  motion  of  link  one,  the  base.  The  seeond  and  third 
columns  multiply  by  the  time  rate  of  change  of  gimbal  velocities,  and  account  for  the 
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additional  rotation  of  each  link  due  to  the  motion  of  the  gimbal.  Still  referenced  to  an 
arbitrary  general  frame,  this  results  in  Equation  (3.32),  where  each  element  is  a  3x3 
matrix,  resulting  in  a  9x9  matrix. 

'I,  0  O' 

1=1^  /^Ei  0  (3.32) 

h  Ar.  If, 

The  most  convenient  frame  for  the  /  matrix  is  the  body  frame  for  each  row.  Row 
one  will  be  standardized  to  the  body  frame  of  link  one,  row  two  to  link  two,  and  row 
three  to  link  three.  Each  3x3  inertia  matrix  is  natively  in  the  correct  frame,  but  the  first 
column  requires  a  DCM  to  convert  ct\  (that  each  row  will  be  multiplied  by)  to  the  correct 

frame.  Since  the  dynamics  variables  and  joint  forces  are  multiplied  through  each  row 
during  matrix  multiplication,  and  each  row  requires  a  different  frame,  DCMs  cannot  be 
applied  directly  in  the  state  vector.  Therefore,  DCMs  required  to  convert  the  dynamics 
variables  and  joint  forces  will  also  be  set  in  place  in  the  dynamics  submatrices.  Similarly, 
the  E  terms,  which  become  E 0  when  matrix  multiplication  is  performed,  are  naturally 
given  in  the  outer  link  frame.  Therefore,  DCMs  must  be  applied  here  as  well.  This  results 
in  the  form  of  the  /  matrix  for  computation  as  shown  in  Equation  (3.33). 

'I,  0  O' 

1=  ifC'  0  (3.33) 

/3^C'  If, 

A  9x3  matrix  of  zeros  follows  the  /matrix,  since  the  time  rate  of  change  of 
positional  velocity  has  no  impact  on  the  attitude  equations  of  motion. 

The  P  matrix  converts  the  gimbal  constraint  forces  to  torques.  Each  row  of  the 
matrix  has  a  moment  arm  term  for  each  gimbal  it  is  connected  to  and  the  sign  of  the 
element  ensures  proper  alignment  of  and  relative  to  the  link  frame.  Each 

element  of  the  P  matrix  in  Equation  (3.34)  is  a  3x3  matrix,  resulting  in  a  total  size 
of  9x6. 


(3.34) 


The  rows  of  the  P  matrix  must  also  be  put  into  the  relative  link  body  frame. 
While  eaeh  r  term  is  already  in  the  native  frame,  the  gimbal  eonstraint  forees  that  these 
terms  will  be  multiplied  by  are  defined  in  the  inertial  frame  and  must  therefore  be 
eonverted. 

0 

P=  (3.35) 

0 

The  n  matrix  collects  the  cross  product  terms  generated  from  the  gimbal 
constraint  described  in  Equations  (2.22)  and  (2.23).  Each  row  correlates  to  a  link.  The 
first  column  multiplies  by  d)^  and  accounts  for  the  translational  acceleration  due  to  the 
rotation  of  link  one  relative  to  the  inertial  frame.  The  second  and  third  columns  account 
for  the  translational  acceleration  due  to  the  gimbal  rotations.  Each  element  of  the  fl 
matrix  in  Equation  (3.36)  is  a  3x3  matrix,  resulting  in  a  9x9  El  matrix. 

0  0  O' 

n=  W2(f2i-fjj)  0  (3.36) 

The  Tt  matrix  is  best  evaluated  in  the  inertial  frame,  but  this  requires  the  use  of 
DCMs  to  convert  the  various  vector  quantities  to  the  correct  frame.  Eirst,  each  term 


must  be  converted  to  the  inertial  frame.  Eurthermore,  the  E  terms,  which  become  Yfi- 
when  matrix  multiplication  is  performed,  are  in  the  link  two  and  three  frames,  and  must 
also  be  converted  to  inertial.  Easily,  the  d)^,  0^,  and  62 terms  that  will  be  multiplied 


through  need  to  be  transformed  into  the  inertial  frame.  This  results  in  the  final  fl  matrix 
in  Equation  (3.37). 


n  = 


0 

m2{  C  rji  -  C  rji)  C 
m^i  C  Tji  -  C  Til  +  C  r32  -  C  r22) 


0  0 

m2’'C\r2x^,)  0 

m2^C\r2,Y,  +CC\)Y,  -^E,)  m2^C\Y2_ 
(3.37) 


The  fj.  matrix  collects  the  mass  terms. 
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(3.38) 


»ij 

1M2 

1M3 

Each  element  of  Equation  (3.38)  is  a  3x3  diagonal  matrix,  where  the  mass  of  each 
link  is  copied  across  the  diagonal.  The  off-diagonal  terms  are  zero,  as  shown  in 
Equation  (3.39).  This  results  in  a  9x3  matrix. 

mj  0  0 

0  Wj  0 

0  0  m, 

mj  0  0 

// =  0  0  (3.39) 

0  0  mj 

m3  0  0 

0  m3  0 

0  0  m3 

While  the  mass  terms  are  agnostie  to  the  frame,  the  aeceleration  of  link  one  that 
will  right  multiply  the  mass  terms  will  need  to  be  transformed  to  the  inertial  frame,  as 
shown  in  Equation  (3.40). 

"m, 

//=  (3.40) 

m/C' 

Eastly,  the  J  matrix  colleets  the  gimbal  constraint  forces  to  be  used  in  the 
translational  equations  of  motion.  Each  element  of  Equation  (3.41)  is  a  3x3  matrix,  where 
the  diagonal  is  populated  by  either  zeros,  or  positive  or  negative  unity,  and  the  off- 
diagonal  terms  are  zero,  as  shown  in  Equation  (3.42).  The  sign  of  each  element  accounts 
for  proper  orientation  of  the  gimbal  constraint  forces  relative  to  each  link.  Since  the 
constraint  forces  are  defined  in  the  inertial  frame,  no  DCMs  are  necessary. 

■  7  O' 

J  =  -1  1  (3.41) 

0  -1 


37 


1  0  0  0  0  0 

0  1  0  0  0  0 

0  0  1  0  0  0 

-10  0  10  0 

0  -1  0  0  1  0  (3.42) 

0  0  -1  0  0  1 

0  0  0  -1  0  0 

0  0  0  0  -1  0 

0  0  0  0  0  -1 

Similarly,  the  constraint  vector  in  Equation  (3.7)  can  be  partitioned  into  two 
subvectors,  F  and  T  .  The  constraints  for  the  orientation  equations  of  motion  are 
collected  in  T ,  shown  in  Equation  (3.43)  where  each  row  represents  a  3x1  column 
vector,  resulting  in  a  9x1  column  vector. 

r  =  <  +  >(3.43) 

+  7)52  + 1 + 1 I ^6^)- 1 

Similar  to  the  fl  or  E  submatrix  of  the  dynamics  matrices,  each  row  of  the  T 
vector  corresponds  to  a  link,  and  the  local  link  frame  is  utilized  as  the  reference  frame  for 
each  row.  Therefore,  DCMs  are  required  to  transform  gimbal  torques,  ,  as  well 

as  some  of  the  dynamics  terms  involved  in  gimbal  rotation  (Ej^j)  . 


T  =  \  T,+T^,-  ^C%,  -  4  {1,0},)-  1, 

T,  +  T^,  -  m,  {1,(0,)  +  I,CC^t^0^)  +  l,{t,0,)-  I,CCd),  ^C^Y,0,)-  I,CC^T^0^  x  Y,0,) 

(3.44) 

The  constraints  for  the  translation  equations  of  motion  are  collected  in  F  , 
shown  in  Equation  (3.45)  where  each  row  represents  a  3x1  column  vector,  resulting  in  a 
9x1  column  vector. 
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F  = 


E 


-m2r2^yOy  -  m2r2i^iT^0  -  +  m2C02f2i  +  ^2 


(3.45) 


^3(^21^!  ^22^1  +  ^2^1)^!  ^3(^32^!  "*"  ^1^1  \^\ 

...  —  2^P  ~  ^3^32 (^1^1  ^  ^ 2^P'" 

...  —  —  0)2^21  "*"  ^2^22  ~  ^3^32)  "*" ^3 

Similarly  to  the  T  subvector,  each  row  of  the  F  vector  corresponds  to  a  link. 
However,  since  the  equations  of  motion  for  translation  were  placed  in  the  inertial 
reference  frame  in  the  dynamics  submatrices,  F  must  also  be  given  in  the  inertial  frame. 
This  results  in  the  need  for  a  variety  of  transformations  since  each  term  except  the  link 
forces  are  defined  in  local  link  frames. 


F= 


F 

-m,  1  -r^jf  1  +  "C*4f  1)^  ->rhCCr,2  ‘C"  -  'Cq 

...-m,V?, 2^202  -w,  Vr32(^c'q‘c'r24)-w,"c®?32(^c'r,^ 

...  +  V^r23-V^r32)+i^3 


(3.46) 


Reassembling  these  submatrices  and  subvectors  back  into  the  mass  matrix  and 
constraint  subvector  results  in  the  a  form  of  Equation  (3.7),  that  is  relevant  for  numerical 
simulation: 
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A 

/3'C' 

0 


i,r, 

A'cY, 

0 

2"c^(A.r,) 

2/^3": 


m3''cAAir,  +  (^c^A2)r,-A2r,) 


0 

0 

r„‘C" 

0 

0)^ 

0 

0 

-r3/C" 

r  ^C" 
'22  ^ 

0, 

Ar2 

0 

0 

-P 

'^32  '-' 

< 

02 

> 

0 

m^ 

1 

0 

V, 

0 

m  2 

-1 

1 

Fo. 

/M3 

0 

-1 

.As2  . 

A  -A;i  -®i(A®i) 

7;  +7’g,  -  'CYq3  -o>3(73®2)-73(r,0,)-73'C‘o>i‘CYj0, 

7;  +  -  «3  (73®3)  +  73('c'f  j^,)  +  73(f 3^)  - hCc^Q),  ‘c'r^^)  -  x 

A 

-m3  "C'r3,f  ,(9j  -  m3  "C'rji  'C'®;  ‘C'r,(9j  -  m3  +  m3  +  F^ 

-m,''C\r^j',-r^^t,  +  ,)d^  -  m,''C\r^,^C'd),'C^  -  ^0,  + ... 

...-m,''C"P,,CC'a),'C"r^d^)-m,''C^P,^CC^T,d^xT,0,)-m,CC'o},r,^  -  ^ '' C^a^r,^)  +  F, 


(3.47) 


40 


D, 


ELIMINATING  JOINT  CONSTRAINT  FORCES 


While  the  submatriees  and  subveetors  diseussed  are  eonvenient  for  a  greater 
understanding  of  the  interaetions  between  terms  and  are  useful  for  building  the  system 
through  inspeetion,  further  partitioning  must  be  eompleted  before  they  ean  be  effieiently 
solved.  In  order  to  reduee  the  eomplexity  of  the  system,  it  is  desired  to  perform  matrix 
operations  to  eliminate  the  joint  eonstraint  forees,  as  knowledge  of  these  forees  is  not 
required  to  simulate  the  dynamies.  They  ean,  however,  be  reeonstrueted  later  if  required. 

To  perform  the  reduetion  operation,  the  state  veetor,  mass  matrix,  and  eonstraint 
veetor  are  re-partitioned.  The  state  veetor  is  split  between  the  dynamies  variables  and  the 
gimbal  eonstraint  forces.  The  dynamics  variables  are  collected  in  the  x  column  vector  in 
Equation  (3.48),  where  each  term  is  a  3x1  column  vector,  resulting  in  a  12x1  column 
vector. 

(hj 

4 
4 

E 

The  remaining  gimbal  constraint  forces  are  collected  in  the  column  vector  in 

Equation  (3.49),  where  each  term  is  a  3x1  column  vector,  resulting  in  a  6x1  column 
vector. 


(3.48) 


(3.49) 


The  mass  matrix  is  next  split  into  the  A,  R ,  S ,  and  U  submatrices.  The  A 
submatrix  collects  the  first  12  rows  of  the  first  12  columns  of  the  mass  matrix.  Therefore, 


it  collects  the  entire  /  submatrix  and  corresponding  zero  matrices,  as  well  as  the  first 
three  rows  of  the  fl  and  ju  submatrices.  The  A  submatrix  is  shown  in  Equation  (3.50), 


where  each  term  is  a  3x3  matrix,  resulting  in  a  12x12  matrix. 
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(3.50) 


I,  0  0  0 

0  0 

i,T,  0 

0  0  0 


The  R  submatrix  collects  the  remaining  six  rows  of  the  first  12  columns  of  the 
mass  matrix.  Effectively,  it  collects  the  P  matrix  and  first  three  rows  of  the  J  matrix,  as 
shown  in  Equation  (3.51),  where  each  term  is  a  3x3  matrix,  resulting  in  a  12x6  matrix. 


R  = 


r  C 

'll  ^ 


-r  C 

'21 


2/^N 


0 

1 


'll 


0 

ICN 

3/^N 


-r  C 

'll  ^ 


(3.51) 


The  S  submatrix  collects  the  remaining  6  rows  of  the  first  12  columns  of  the 
mass  matrix.  It  collects  the  rows  of  fl  and  /u  not  contained  in  the  A  matrix,  as  shown  in 
Equation  (3.52),  where  each  element  is  a  3x3  matrix,  resulting  in  a  6x12  matrix. 


c  ^21  C  Tjj) 

iV^l 


iT,) 


0 


«^3(  C  ^21  -  +  "CT32  -  C"r22)  m2C'?22^2 


m. 


(3.52) 

Easily,  the  U  matrix  collects  the  remaining  6  rows  of  the  remaining  6  columns  of 
the  mass  matrix.  It  collects  the  J  terms  not  included  in  the  R  matrix,  as  shown  in 
Equation  (3.53),  where  each  element  is  a  3x3  matrix,  resulting  in  a  6x6  matrix. 


t/  = 


-1 

0 


1 

-1 


(3.53) 


Since  the  dynamics  and  state  vectors  were  partitioned  as  described,  the  constraint 
vector  must  also  be  restructured  into  the  T'  and  F'  subvectors.  The  T'  subvector 
collects  the  first  12  elements  of  the  constraint  vector,  or  the  entire  T  vector  and  first 
three  elements  of  the  F  vector,  resulting  in  a  12x1  column  vector,  shown  in 
Equation  (3.54). 
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^2+^01  “ 

T,  +  -  ®j  (/,®,)  +  h{WA)  +  h(tA) 

F, 

The  F'  subvector  collects  the  remaining  constraint  terms,  or  the  remaining  six 
elements  of  the  F  vector,  resulting  in  a  6x1  column  vector,  shown  in  Equation  (3.55). 

F'  =  -m,  +  CC^r,2)t,)0,  -  m,  (3-55) 

...  -  -  ^2^21  +  ®2^22  “  +  ^2 


When  assembled,  the  A,R,S  and  U  submatrices  yield  Equation  (3.56),  expanded 
for  clarity  in  Equation  (3.57). 


'A 

*5  [F'J 


(3.56) 
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A 

7 


R 

U 


r  h 

0 

0 

0 

'll  ^ 

0 

'  6\' 

73*C' 

7:7 

0 

0 

rX‘C^' 

Gi 

73 -r' 

73^C^F. 

77 

0 

0 

02 

0 

0 

0 

»»1 

1 

0 

m.C'CX-^'CX) 

0 

-1 

1 

7i 

mA  C  '  c  ~ 

^'C%)  W3  -'C* (r,iFi  +  (‘CV3,)Fi  -  ?,,Fi) 

m- 

0 

-1 

foi] 

^1  A  ) 

r,  +  Tsi  -  -  a.  (/,a»,) 

Jj  +r<,,  -d>3(/36^)+/3('c*ri4)+73(r,4)-/3('c'fi\'c-’r,^,)-73('c*ri4  xr,4) 

_ _  _ _ _ ^i_ _ ^ _ _ _ 

-nu  ■''  CV^iFi^i  -  w,  ■'  C‘p2i  ‘C'^i  'C'Fi^  -  w, +  m.  ''C’ojr,!  +  7s 
m,  •'■C*(?,ifi  -  r,,fi  +  ^C%ri)&i  -  m,  'C'cS,  'C*  -^s,  ‘C'<Si  'C‘)Fi(9i  -  m,  ^'C%hA  +  ■■■ 

...  -  m.  -  m.  -'C^C^C-Fi^  x  Fj^)  -  Wj  (■'''C'Sir„  -  •'C*©,?-,!  +  -'C-a/Sj  -  •'■C-'S3r3j)  +  Fj 


(3.57) 
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(3.58) 


Multiplied  out,  Equation  (3.56)  gives: 

Ax  +  RF^=r 

Sx  +  UF^=F.  (3.59) 

Solving  Equation  (3.59)  for  Fq  yields: 

Fq=U-\F'  -Sx) .  (3.60) 

Reinserting  Equation  (3.60)  back  into  Equation  (3.58)  yields  a  form  where  F^  has 
been  eliminated  as  an  independent  variable. 

M  +  R{U~\F'-Sx))  =  T'  (3.61) 

Consolidating  terms  yields: 

{A-RU~'S)i  =  r-RU~'F' .  (3.62) 

Easily,  Equation  (3.62)  can  be  solved  for  x  to  yield  the  values  of  the  desired 
variables,  Q\,  0^,  9^,  and  v  as  shown  in  Equation  (3.63). 

(hj 

0 

.1  \  =  {A-RU-'Sy\r-RU-'F')  (3.63) 

^2 

^1. 

Equation  (3.63)  is  the  final  deliverable  for  the  dynamic  simulation  model.  Eor  the 
general  three-body  model,  the  other  state  variables  ( £U, ,  ,  tUg ,  q ,  q,  9^,  9^,  9^,  9^^,  v , 

d)  can  be  solved  for  by  augmenting  the  state  vector  and  integrating  Equation  (3.63).  A 
MATEAB  script  was  developed  and  utilized  to  solve  for  these  values,  as  described  in 
Chapter  IV. 
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IV.  IMPLEMENTING  THE  SIMULATION  IN  MATLAB 


A,  GENERIC  MATLAB  CODE 

In  order  to  perform  simulations  of  the  system,  the  equations  from  Seetion  2.3 
were  implemented  in  a  general  MATLAB  code.  The  code  was  written  for  any  three-body 
gimbaled  system  as  previously  configured,  and  as  written  in  such  a  way  as  to  facilitate 
easy  tailoring  to  various  implementations  of  the  described  system,  (i.e.,  different  gimbal 
or  body  constraints). 

The  code,  provided  in  Appendix  A,  uses  a  symbolic  architecture  that  utilizes 
numerical  data  for  constants  or  knowns  such  as  mass,  lengths,  inertias,  etc.,  and  uses 
placeholder  symbols  for  unknowns  or  variables  expected  to  change  such  as  gimbal  angles 
and  rates.  The  equations  and  matrices  are  then  solved  symbolically  to  yield  a  final  x  in 
terms  of  the  symbolic  variables.  However,  this  vector  only  includes  the  state  variables, 
X  =  .  In  order  to  step  the  simulation  forward  in  time,  the  state  vector  must 

be  expanded  to  describe  the  entire  set  of  state  variables  for  the  system  under 
consideration.  Furthermore,  q  is  reestablished  as  the  column  vector  of  four  quaternions, 
and  d  is  introduced  as  a  3x1  column  vector  to  describe  the  translation  of  link  one. 


With  these  variables  established,  a  new  state  vector,  x' ,  is  defined  in 
Equation  (4.1). 


ry, 

4 

0, 

di 


(4.1) 


Taking  the  derivative  of  x'  yields  Equation  (4.2). 


47 


(^I 

4 

A 


The  augmented  state  veetor  is  used  in  the  MATLAB  funetion  ODE45  along  with 
initial  values  for  eaeh  state  and  a  start  and  end  time.  The  MATLAB  ODE45  funetion 
integrates  the  system  over  the  specified  timeframe  and  returns  a  time  history  of  the  states 
in  x' . 

It  was  found  early  in  the  development  of  the  MATLAB  Code  that  the  symbolic 
implementation  is  an  inefficient  approach  for  solving  the  matrix  equations.  The  first 
MATLAB  code  was  written  to  provide  equations  of  motion  in  purely  symbolic  terms, 
even  those  where  numerical  values  can  be  expected  as  knowns  (i.e.,  mass,  lengths, 
inertias,  etc.).  However,  it  was  found  that  with  such  large  matrices  and  with  so  many 
symbolic  variables,  MATLAB  was  unable  to  compute  the  symbolic  solution  on  the 
available  hardware.  The  number  of  symbolic  variables  was  reduced  by  utilizing  values 
for  known  quantities  and  relegating  symbolic  use  to  only  unknown  variables.  Lurther 
order  reduction  of  the  system  was  made  by  implementing  gimbal  or  link  constraints.  This 
effectively  reduces  the  system  dimensionality  to  less  than  3N+3,  driving  the  system  to 
become  solvable  and  far  more  efficient.  However,  if  a  large  order  system  is  required,  one 
with  full  range  of  motion  in  each  gimbal  or  with  a  large  number  of  links,  for  example,  a 
numerical  approach  is  recommended  due  to  the  complexities  of  the  symbolic 
computation. 

This  more  efficient  numerical  approach  was  utilized  by  LCDR  Rich 
Gargano  [19].  Whereas  the  method  described  here  solves  for  the  time  rate  of  change  of 
the  state  variable  as  an  equation,  then  passes  these  equations  to  ODE45  to  be  solved, 
Gargano  solves  the  matrix  equations  within  ODE45  numerically.  Gargano  utilizes  a 
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dynamics  function  within  ODE45  to  complete  the  matrix  calculations.  Initial  values  are 
passed  to  the  dynamies  equation,  whieh  populates  the  matriees  with  numerieal  values, 
thereby  avoiding  the  use  of  symbolie  variables.  The  dynamies  equations  are  then  solved 
numerieally.  MATLAB  is  able  to  solve  the  numerieal  matriees  far  more  effieiently  than 
the  symbolie  matrices,  resulting  in  orders  of  magnitude  improvement  in  computational 
efficiency. 

B,  SYSTEM  TAILORING 

In  order  to  troubleshoot  the  MATLAB  code  during  development,  and  in  order  to 
eomplete  validation  and  verifieation  of  the  eode  and  the  derived  Newton-Euler  equations, 
the  general  system  was  tailored  to  deseribe  a  simple  two-link  pendulum. 

One  of  the  benefits  of  the  approaeh  deseribed  in  [15]  is  the  ease  of  tailoring  the 
general  equations  to  a  speeified  system.  Eirst,  by  manipulating  the  strueture  of  the 
/,n,//  and  partieularly  the  P  and  J  matriees,  the  layout  of  the  system  ean  be  easily 
altered.  Eor  example,  body  three  could  be  moved  to  eonneet  to  body  one  instead  of  body 
two.  This  approaeh  is  eovered  in  [15]  for  both  the  spherical  joint  and  gimbaled 
procedures  and  should  be  implemented  when  building  the  submatrices.  The  other  way  to 
tailor  the  system  is  to  eonstrain  body  motion  or  gimbal  motion.  Eor  example,  body  one 
could  be  eonstrained  in  one  or  more  axes  for  translation  or  attitude,  or  eertain  gimbal  axes 
eould  be  eonstrained  to  yield  a  double  or  single  axis  gimbal,  as  opposed  to  the  three  axes 
gimbal  utilized  in  the  general  model.  Constraints  could  be  implemented  when  building 
the  submatriees  but  it  was  found  to  be  easier  to  eonsider  the  full  system,  then  apply 
constraints  to  the  A,R,S,  and  U  matrices.  Sinee  the  basic  layout  of  the  general  ease  is 
applieable  to  the  desired  test  eases,  delaying  eonstraint  application  as  late  as  possible 
allowed  for  the  easiest  tailoring  to  eaeh  specific  system,  while  minimizing  ehanges  to  the 
default  eode,  and  thus  minimizing  the  possibility  of  introdueing  errors.  However,  the 
earlier  these  eonstraints  are  implemented,  the  more  effieient  the  eode  is  sinee  operations 
are  not  being  performed  on  superfluous  variables.  Nonetheless,  the  operations  that  would 
be  eliminated  by  implementing  constraints  early  are  simple  operations  that  MATLAB  ean 
perform  quickly.  The  largest  savings  are  aehieved  by  constraining  the  system  prior  to 
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taking  the  inverse  of  any  matriees.  Waiting  to  eonstrain  the  system  until  the  A,R,S,  and 
U  matriees  are  eonstructed  minimizes  the  effieieney  losses,  while  maintaining  an  easy  to 
tailor  system. 

To  tailor  the  system  using  eonstraints,  the  axes  of  movement  for  body  one  and  the 
axes  of  movement  for  each  gimbal  must  be  investigated.  For  example,  if  the  motion  of 
joint  one  is  constrained  in  the  x-axis  to  create  a  two-axes  gimbaled  joint  in  the  y  and  z 

axes,  then  the  state  variables  9^^  is  constant  and  9^  and  9^  are  both  zero  by  definition. 

To  begin  the  tailoring  process,  the  impact  the  constrained  variable  would  have  on 
other  equations  of  motion  if  unconstrained  must  be  eliminated.  This  is  completed  by 
replacing  the  angle  or  velocity  symbol  for  the  constrained  axis  with  a  static  numerical 
value  or  zero,  respectively,  then  proceeding  with  the  simulation  as  normal.  For  example, 
if  body  one  is  considered  inertial,  body  rates  for  ft}  are  set  to  zero,  resulting  in  zeros  for 

ft}  and  o\ .  Therefore,  any  terms  multiplied  by  these  matrices  will  be  set  to  zero,  thus 
eliminating  the  impact  ft}  would  have  on  the  motion  of  the  other  links.  Then  the 
quaternions  vector  for  body  one,  q,  would  be  set  to  some  numerical  value  to  indicate  a 
constant  orientation  of  body  one,  and  q  is  set  to  a  constant  {0,0, 0,0}  to  negate  rotation. 

Next,  the  system  of  equations  must  be  reduced  so  that  the  zeroed  variables  are  not 
solved.  Continuing  with  the  constraint  example,  and  starting  with  Equation  (3.56),  it  can 

be  seen  that  any  column  multiplied  by  9^^  will  be  zero  since  motion  is  constrained  in  that 

axis.  Since  9^^  is  the  fourth  element  in  i  ,  it  can  be  eliminated  along  with  the  fourth 

column  of  the  A  and  S  submatrices.  However,  this  creates  a  problem  in  the  size  of  the 
system  of  equations. 

With  the  constrained  columns  eliminated,  the  system  becomes  overdetermined 
since  the  row  corresponding  to  the  eliminated  constraint  acts  as  a  redundant  constraint 
equation  for  the  gimbal  torque.  In  order  to  re-create  a  square  syste,  an  equivalent  number 
of  rows  must  be  eliminated  in  order  to  drive  the  number  of  equations  equal  to  the  number 
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of  unknowns.  However,  an  additional  procedure  must  be  implemented  first  in  order  to 
retain  the  joint  constraint  terms  that  will  be  eliminated  in  those  rows. 

For  unconstrained  joint  axes,  the  torque  is  treated  as  a  control  input  and  provided 
by  the  user.  However,  for  constrained  axes,  an  internal  resultant  torque  is  generated  in  the 
joint  to  counteract  any  torques  applied  to  the  outer  link.  This  can  be  easily  implemented 
in  the  equations  by  replacing  the  total  joint  torque  terms  (Tqj,Tq2)  with  a  combination  of 

joint  control  torques  (Jqic^^gic)  and  joint  reaction  torques  (Jqir,Tq2r)  •  Each  axis  of  the 
total  joint  torque  term  will  have  either  a  control  torque,  or  a  reaction  torque,  but  cannot 
have  both.  It  is  important  to  note  that  in  [15],  joint  torque  also  includes  damping  terms. 
However,  for  the  scope  of  this  thesis,  damping  was  assumed  minimal  and  not  modeled. 


For  example,  consider  a  free  floating  link  one,  free  to  move  in  all  six  degrees  of 
freedom  (DOF),  attached  to  a  link  two  by  a  two  axes  gimbal,  free  to  move  in  the  x  and  z 
axes,  but  constrained  in  the  y-axis.  In  practice,  a  torque  would  be  applied  in  the  x  and  z 
axes  to  drive  link  motion.  Therefore,  the  torque  in  the  x  and  z  axes  would  be  comprised 
of  a  control  torque.  However,  the  internal  torque  in  the  y-axis  is  driven  by  the  y-axis 
torques  exerted  by  the  second  link.  This  would  generate  a  reaction  torque  for  the  y-axis  of 
the  joint.  Suppose  an  external  torque  is  applied  to  link  two  in  the  y-axis.  This  torque 
would  not  drive  motion  in  the  gimbal,  as  it  is  constrained  and  cannot  be  moved  in  the  y- 
axis.  However,  the  external  torque  does  generate  a  resultant  torque  at  the  gimbaled  joint 
that  translates  from  link  two,  through  the  joint,  to  link  one,  and  will  drive  motion  in  link 
one.  This  reaction  torque  would  be  the  combination  of  the  external  torque,  and  any 
torques  generated  by  the  motion  of  link  two.  In  this  case,  the  total  torque  in  gimbal  one 
would  be  as  follows: 


TG\  =  \ 


^GlCx 


'^G\Ry 


‘GlCz 


(4.3) 


In  order  to  implement  propagation  of  torques  from  outer  links  to  inner  links  via 
resultant  torques,  the  resultant  torque  must  first  be  solved.  In  the  case  of  joint  one, 
starting  with  Equation  (3.5)  and  exploring  only  the  y-axis  terms  yields  Equation  (4.4). 
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(4.4) 


- T2y+  \iRy-\2y- [4 (4^2)]^  - [^r  1^1]^  - ,e\^ 

This  is  then  solved  for  . 

"^Gliiy  “[^2^1]^,  '^\_^  2^ ~[^21^Gl]v  '*'[^22^02];,  “^Jy+T^jy--- 
...  +  [«2  (/2®2  )]y  +  [^2r  1^1]^  -  [/2®ir  1^]^, 

The  result,  given  in  Equation  (4.5),  can  then  be  inserted  in  to  the  y  component  of 
Equation  (3.4),  as  shown  in  Equation  (4.6) 

[IA]  +  %Fgi]=T,-{[I20\\  +[Wi]  -[^21^Gl]y  +[4^G2]y  -T2y  +  TG2y... 

(4.6) 

•••+[^(^2«>)]y+[^2ri4]^,-[Wi^p-[^(A^^)] 


[(A  +  Ay  )®i] + [Ar  A  ]  +  [(A  -  ^21  y)A;i  ] + [  AA?2  ],,  =  - 

.  .  .  (4.7) 

-A  +  Ay-  TG2y-  [«2  (  A^^2  )],  “  [A^  Al  “  [^1  (  A^^l  )]  “  [A^A  A\ 

In  practice,  the  process  of  reducing  the  system  order  can  be  implemented 
anywhere  from  deriving  the  equations  of  motion  to  equation  to  the  construction  of  the  A 
R  and  T’  matrices  [15].  It  can  be  noted  that  the  process  of  solving  for  a  resultant  torque 
and  applying  it  to  the  inner  body  is  effectively  accomplished  by  adding  the  constrained 
equation  of  motion  of  the  outer  to  the  inner  body  equations  of  motion.  Propagating  the 
terms  is  as  simple  as  converting  the  constrained  row  of  the  outer  body  to  the  inner  body 
frame  using  a  DCM  and  adding  the  result  to  the  corresponding  inner  body.  This  is 
illustrated  in  Equation  (4.8),  where  the  subscript  i  denotes  the  three  rows  of  the  inner 
body  and  the  subscript  o  denotes  the  constrained  outer  body  row.  It  is  important  to  note 
that  a  row  populated  by  zero  must  be  added  to  the  constrained  outer  body  row  for  each 
axis  that  is  not  constrained  (i.e.,  if  an  axis  is  controlled  instead  of  constrained,  the  row  is 
replaced  with  zeroes)  in  order  to  give  it  the  proper  dimensions  for  the  DCM. 

{A  -  RU-^S),  +  ‘  C°{A  -RU  'S)^  =  {T  - 7? 6/ A').  +  ‘  -  RU  'F')^  (4.8) 

Eor  example,  start  with  the  general  system  previously  described  and  implement 
constraints  in  Equation  (3.62).  Suppose  gimbal  two  is  constrained  in  the  x-axis,  meaning 
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0^^,  6^^,  and  6-^^  are  all  zero  by  definition.  Therefore,  column  four  of  the  {A-RU  ^5') 

matrix,  which  corresponds  to  ^i;,can  be  eliminated  since  it  multiplies  by  zero.  Then,  row 

four,  which  corresponds  to  the  motion  of  link  two  in  the  x-axis,  which  is  now  a  torque 
constraint,  can  be  added  to  row  one  through  three,  which  corresponds  to  the  rotation  of 
link  one.  While  the  constrained  row  in  the  outer  body  may  only  be  a  single  row,  once  the 
necessary  DCM  is  applied  to  convert  it  to  the  inner  body  frame,  the  terms  may  apply  to 
all  three  rows  of  the  inner  body. 

{A - RU-^S\_,  +  ^C\A- RU-^S),  =  {T'  - RU-'F'\_,  +  '  C\r  - (4.9) 

Furthermore,  if  more  than  one  outer  link  is  constrained  in  the  same  axis,  both 
constrained  rows  will  need  to  be  propagated  inward,  starting  with  the  outermost  link.  For 
example,  start  with  the  general  case  and  assume  gimbals  one  and  two  are  both 
constrained  in  the  x-axis.  The  seventh  row  (x-axis  rotation  of  link  three)  is  left  multiplied 
by  a  DCM  to  convert  it  to  the  body  two  frame,  then  added  to  rows  four  through  six.  Then 
row  four,  which  now  also  includes  terms  from  the  link  three  equation  of  motion,  is  left 
multiplied  by  a  DCM  to  convert  it  the  body  one  frame,  and  added  to  rows  one  through 
three. 

{A  -RU-'S),_,  +  ^C\A  -RU-'S),  =  {T  -  FD '  C'(r'  -  Ff/  'F'),  (4.10) 

(^-FD-'%3  +  'C^((^-FD-‘5),  +  ^C"(^-F[F'5),)=...  ^ 

...(F  -RU-^F),_^  + '  C^((r  -RU-^F\  + "  C'(r  -Rir'F\) 

Once  this  addition  is  complete  and  the  terms  are  propagated  to  the  innermost  free 
link,  the  redundant  outer  row  can  then  be  eliminated,  resulting  in  a  properly  sized  system 
of  equations. 

This  process  is  repeated  for  any  constrained  gimbal  axes,  by  eliminating  columns 
and  combining  and  eliminating  rows  as  necessary  to  eliminate  gimbal  motion  and 
translation  or  rotation  of  link  one. 

The  elimination  of  translation  or  rotation  of  link  one  is  more  straightforward.  As 
described,  the  columns  that  are  multiplied  by  rq  and  Vj  are  eliminated  as  usual. 
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However,  since  link  one  is  the  innermost  body,  the  additional  process  described  for 
constrained  gimbals  does  not  apply.  Link  one  is  instead  modeled  as  an  immovable  point 
in  space.  The  joint  force,  Fgi  is  solved  in  such  a  way  as  to  lock  the  joint  to  inertial  space. 
Another  way  to  view  this  setup  is  to  assume  the  external  torques  and  forces  on  link  one 
are  dynamic  and  always  have  the  proper  orientation  and  magnitude  to  keep  the  link 
stationary.  Therefore,  the  rows  that  correspond  to  the  eliminated  degrees  of  freedom  for 
body  one  can  simply  be  eliminated. 
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V.  VALIDATION  AND  VERIFICATION 


A,  SIMMECHANICS  AS  A  VALIDATION  AND  VERIFICATION  TOOL 

In  order  to  validate  the  accuracy  of  the  MATLAB  model  developed  in 
Chapter  IV,  independent  tools  were  used  to  check  the  dynamics  equations  and  overall 
system  response.  The  SymPy  program,  which  utilizes  Python  scripts  and  system 
configuration  data  and  inputs,  was  utilized  to  generate  equations  of  motion  for  each 
model.  The  equations  were  checked  against  symbolic  outputs  from  MATLAB  in  order  to 
verify  no  dynamics  components  were  missing.  This  was  particularly  vital  to  identifying 
and  correcting  errors  in  that  specific  terms  could  be  identified  and  corrected.  The  other 
tool  used  for  this  was  the  SimMechanics  software.  SimMechanics  is  a  MATLAB 
environment  that  allows  for  easy  construction  and  simulation  of  multibody  systems.  The 
program  uses  a  block-diagram  approach  which  allows  the  user  to  add  bodies,  joints,  and 
forces,  and  torques  in  any  desired  configuration.  The  program  then  evaluates  the  system 
dynamics,  solves  the  equations  of  motion,  and  propagates  the  dynamics  for  a  specified 
period.  Lastly,  the  program  generates  an  animation  of  the  system  in  order  to  visualize  the 
results. 

SimMechanics  was  utilized  because  it  is  simple  to  learn  and  operate,  flexible 
enough  to  facilitate  construction  of  a  wide  variety  of  system  configurations,  and  operates 
natively  with  MATLAB,  providing  for  easy  output  and  correlation  of  results  with  the 
developed  MATLAB  model.  One  drawback  to  the  SimMechanics  suite  is  that  it  solves 
for  the  dynamics  directly  and  does  not  provide  the  equations  of  motion  as  an  output.  This 
shortcoming  would  make  it  difficult  to  use  the  SimMechanics  results  directly  for 
optimization. 

The  model  is  generated  in  the  standard  Simulink  workspace,  as  shown  in 
Figure  12.  Each  of  the  primary  blocks  represents  either  the  mass  properties  of  a  link,  a 
joint,  or  a  translation  to  properly  configure  the  system. 
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Figure  12.  General  SimMechanics  Model  for  Three  Link  System 


Mass  properties,  displayed  in  Figure  12  as  the  “Link  1  CoM,”  “Link  2  CoM,”  and 
“Link  3  CoM,”  speeify  the  mass  of  each  link  as  well  as  the  moments  of  inertia.  These 
blocks  also  specify  the  shapes  and  dimensions  of  the  links  to  be  used  in  generating  the 
animation.  While  these  shapes  and  dimensions  in  themselves  are  not  utilized  to  solve  for 
the  dynamics,  they  can  be  utilized  to  automatically  generate  appropriate  inertia  tensors. 
This  option  was  not  utilized  for  this  work  and  custom  inertia  tensors  were  manually 
inserted  in  order  to  ensure  properties  matched  those  used  in  the  MATLAB  simulations. 

Each  joint,  labeled  “Joint  1”  and  “Joint  2”  in  Figure  12,  is  specified  by  a 
subsystem  of  blocks.  This  subsystem  contains  three  individual  blocks.  The  main  block, 
labeled  “Joint  1”  or  “Joint  2,”  represents  a  revolute  joint  about  a  single  axis.  While  every 
system  explored  in  this  thesis  consists  of  single  axis  joints,  multiple  blocks  could  be  used 
to  construct  a  dual  or  triple  axis  joint.  One  property  of  the  revolute  joint  is  that  it  always 
rotates  about  the  z-axis.  Therefore,  if  the  joint  is  desired  to  rotate  in  the  x  or  y  axes,  the 
joint  must  first  be  rotated  accordingly.  Two  blocks  in  the  subsystem,  labeled  “Align  Z  for 
Joint”  and  “Unalign  Z  for  Joint”  perform  the  required  90  degree  rotations  about  the  x  or  y 
axes  in  order  to  align  the  z  axis  of  the  joint  to  the  desired  axis  of  the  system,  as  seen  in 
Figure  13.  If  a  z-axis  joint  is  desired,  these  blocks  become  dummy  blocks  and  perform  no 
function. 
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Figure  13.  SimMechanics  Joint  Subsystem 

The  translations  between  joints  and  link  centers  of  mass  are  performed  by  the 
subsystems  labeled  “Rll,”  “R21,”  “R22,”  and  “R32.”  Each  of  these  subsystems  contain 
translation  blocks  for  the  x,  y,  and  z  components  of  the  moment  arms  ( ^11,^20  ^22 ’^32)  ’ 
shown  in  Figure  14. 


Figure  14.  SimMechanics  Translation  Subsystem 

Other  blocks  in  the  primary  workspace  include  the  Solver  Configuration  block, 
which  defines  solver  settings  for  the  simulation;  the  Mechanism  Configuration  block, 
which  establishes  the  global  gravity  acceleration;  the  World  Frame,  which  establishes  an 
inertial  reference  frame;  and  a  6-DOF  Joint,  which  allows  for  translation  and  rotation  of 
link  one  relative  to  the  inertial  reference  frame  (this  block  is  removed  for  a  system  with 
link  one  fixed). 
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B. 


TWO-LINK  PENDULUM  MODEL 


The  first  test  case  utilized  to  debug  the  developed  dynamic  model  and  to  perform 
validation  and  verification  on  was  a  simple  two-link  pendulum  system.  In  order  to  tailor 
the  general  model  to  a  simple  two-link  pendulum,  the  desired  two-link  pendulum  must 
first  be  described.  Link  one  is  fixed  for  motion  in  both  translation  and  attitude.  This 
effectively  removes  the  link  from  the  system,  resulting  in  gimbal  one  being  fixed  to  an 
inertial  point  in  space.  Link  two  and  three  are  modeled  as  rods  connected  by  single  axis 
gimbals,  rotating  in  body  local  z-axes.  Gravity  is  established  along  the  inertial  y  -xis  as 

-9.81'^2  •  Arbitrary  masses,  lengths  and  inertias  are  provided  for  each  link.  This  results 
in  the  system  shown  in  Figure  15. 


First,  the  masses,  lengths,  and  inertias  are  provided  or  calculated  for  each  link. 
The  parameters  for  link  one  can  be  left  as  symbols  since  these  values  will  be  eliminated 
once  constraints  are  applied.  For  the  two-link  pendulum  system,  masses  of  10  kg  were 
assumed  for  link  two  and  three.  Each  link  was  assumed  to  have  a  length,  /,  of  one  meter 
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and  width,  r,  of  10  centimeters,  with  mass  evenly  distributed  so  the  center  of  mass 
resides  at  the  volumetric  center  of  the  link.  Inertias  were  assumed  as  follows; 

—mr^  0  0 

2  [0.050  0  0 

= /3  =  0  —ml^  0  =  0  0.833  0  kg-rn^  .  (5.1) 

.  [  0  0  0.833 

0  0  — mL 

12  ^ 

Constraints  were  applied  in  the  MATLAB  model  starting  with  eonstraining  link 
one  to  the  inertial  frame  as  deseribed.  First,  the  orientation  of  link  one  was  aligned  to  the 
inertial  frame  by  setting  q  =  [0,0,0,!]^.  Next,  the  rotation  of  link  one  was  zeroed  by 
setting  q  =  [0,0, 0,0]^.  This  negated  the  impaet  any  rotation  of  link  one  would  have  on  the 
subsequent  links.  Next,  rows  one  through  three  of  the  A,R,  and  T' submatriees,  as  well 
as  eolumns  one  through  three  of  the  A  and  S  submatriees  were  eliminated  to  eonstrain 
attitude  motion  of  body  one.  Rows  10  through  12  of  the  A,R,  and  T' submatriees,  as  well 
as  eolumns  10  through  12  of  the  A  and  S  submatriees  were  also  eliminated  to  eonstrain 
translational  motion  of  body  one. 

Joint  eonstraints  were  implemented  by  setting 

to  zero,  while  leaving  0^^  as  symbolie  variables.  Lastly,  eolumns 

four,  five,  seven  and  eight  of  the  A  and  S  submatriees  were  eliminated.  Next  rows  seven 
and  eight  (eorresponding  to  the  eonstrained  axes  for  link  three)  were  eonverted  to  the  link 
two  frame  and  added  to  rows  four  through  six  (eorresponding  to  link  two),  as  deseribed  in 
Chapter  IV.  Then  rows  four  and  five  were  eonverted  to  the  link  one  frame  and  added  to 
rows  three  through  four.  Note  that  in  this  ease  the  eonstrained  gimbal  torques  that  get 
propagated  through  are  eventually  eliminated  altogether.  This  is  speeifie  to  a  ease  where 
none  of  the  links  ean  move  in  the  eonstrained  degree  of  freedom.  The  result  is  a  two 
dimension  system  to  be  solved  using  the  Newton-Euler  approaeh,  where  x  was 
simplified  from  to  only  two  variables. 
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(5.2) 


This  system  was  expanded  to  the  full  state  dynamics  model; 


x'  =  < 


l7 


l7 


a 


27 


X  =  < 


^27 

^.7 

0. 


2z , 


(5.3) 


(5.4) 


The  full  state  model  was  then  solved  for  a  10  second  simulation,  where  ^1,^2, 
02  were  initialized  as  zero,  and  0^  was  initialized  at  300°,  as  depicted  in  Figure  16. 


The  resulting  gimbal  angles  and  rates  were  plotted,  as  shown  in  Figure  17. 
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Figure  17.  Double  Pendulum  Results  from  Newton-Euler  Simulation 

The  full  eode  for  the  MATLAB  model  of  the  double  pendulum  can  be  found  in 
Appendix  C. 

Once  the  response  was  obtained  from  the  MATLAB  model,  it  needed  to  be 
verified.  The  general  SimMechanics  system  was  tailored  to  the  double  pendulum  model. 
This  model  can  be  found  in  Appendix  D.  The  simulation  was  then  performed  for  a  10 
second  interval  to  match  the  simulation  that  was  run  for  the  MATLAB  model.  The  results 
of  this  simulation  can  be  seen  in  Ligure  18. 
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Figure  18.  Double  Pendulum  Results  from  SimMeehanies 

Comparing  the  SimMeehanies  results  to  those  of  the  developed  simulation  code,  it 
is  observed  that  the  two  results  are  identical.  Absolute  differences  for  gimbaled  joint 
angles  and  rates  can  be  seen  in  Figure  19.  For  the  double  pendulum  case,  the  differences 
are  seen  to  be  negligible.  Hence,  the  two  models  are  considered  to  be  in  agreement  for  the 
tested  double  pendulum  system. 
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Figure  19.  Double  Pendulum  Simulation  Residuals 

Onee  the  simulations  for  the  two-link  pendulum  eonfirmed  the  viability  of  the 
proeess,  further  tests  were  eondueted  on  a  more  relevant  system  eonfiguration. 

C.  AZIMUTH-ELEVATION  SYSTEM. 

A  more  applieable  system  eonfiguration  to  approximate  a  gimbaled  antenna  is  an 
azimuth-elevation  setup.  In  order  to  maintain  a  stepwise  evolution  of  the  system,  the 
double  pendulum  eonfiguration  was  adapted  to  the  azimuth-elevation  setup.  Properties 
such  as  link  dimensions,  link  mass,  and  link  inertia  were  transferred  directly  from  the 
previous  double  pendulum  model.  The  system  was  easily  converted  to  an  azimuth- 
elevation  system  by  changing  the  first  gimbaled  joint  from  a  z-axis  joint,  to  a  y-axis  joint. 
This  created  an  azimuth  gimbal.  The  second  joint  was  maintained  as  a  z-axis  joint  in 
order  to  establish  an  elevation  joint.  Lastly,  an  inertial  base  was  established,  effectively 
creating  a  two  link  system  where  joint  one  is  fixed  in  inertial  space,  as  seen  in  Figure  20. 
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Figure  20.  Azimuth-Elevation  Configuration 

Since  the  MATLAB  code  was  written  in  a  general  form,  only  small  changes  to  the 
double  pendulum  code  were  required  to  implement  the  az-el  system.  No  changes  were 
necessary  to  link  one,  so  the  body  remained  constrained  in  both  translation  and 
orientation. 

Joint  constraints  were  implemented  by  setting  and  6^2,. 

to  zero,  while  leaving  and  ^2z  symbolic  variables.  Column  elimination 

proceeded  in  the  same  fashion  as  the  double  pendulum,  but  instead  of  eliminating  column 
five,  column  six  was  eliminated,  changing  the  constraint  on  joint  one  from  the  y-axis  to 
the  z-axis.  This  was  accomplished  by  simply  changing  a  few  indices  in  the  MATLAB 
code.  Similarly,  row  addition  was  completed  the  same  as  for  the  double  pendulum,  except 
row  six  was  added  instead  of  row  five.  Unlike  the  double  pendulum  case,  where  an  off- 
axis  reaction  in  joint  two  does  not  create  out  of  plane  motion  about  joint  one,  any  off  axis 
reaction  torque  in  joint  two  for  the  az-el  system  (created  by  the  motion  of  body  three)  has 
the  potential  to  induce  motion  in  body  two,  at  least  about  the  gimbaled  y-axis.  Again,  this 
configuration  change  was  as  simple  as  modifying  a  few  indices  in  the  MATLAB  code. 
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This  resulted  in  the  following  system: 


X  = 


a 


(5.5) 


2z, 


Expanding  this  system  to  the  augmented  state  model  yielded: 


X  =  < 


0, 


1  V 


0. 


2z 


ly 


(5.6) 


The  augmented  model  was  solved  for  a  10  second  simulation,  where  initial 
conditions  on  0^,02  0x,  and  02  were  all  taken  as  zero  (see  Figure  21). 


gm^ 


t  gm3 


Figure  2 1 .  Azimuth-Elevation  Initial  Configuration 
The  resulting  gimbal  angles  and  rates  were  plotted,  as  shown  in  Figure  22. 
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Figure  22.  Azimuth-Elevation  Results  from  Newton-Euler  Simulation 


Since  body  three  has  no  off  axis  inertia  components  in  this  case,  it  does  not 
generate  any  off  axis  torque  about  gimbal  one.  Therefore,  no  reaction  torque  is  created  in 
joint  two,  and  no  movement  is  induced  into  body  two.  While  there  are  joint  reaction 
forces  in  joint  two,  they  are  in  the  x  and  y  axes,  which  are  constrained  in  joint  one,  so 
they  cannot  create  movement  of  body  one  or  body  two.  The  system  thus  operates  similar 
to  a  single  pendulum. 

In  order  to  investigate  proper  propagation  of  off  axis  forces  and  torques  in  the 
code,  arbitrary  off  axis  components  were  added  to  the  inertia  matrix  for  bodies  one  and 
two. 
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No  further  changes  to  the  configuration  or  the  code  were  made.  The  system  was 
then  simulated  for  10  seconds.  The  results  can  be  seen  in  Figure  23. 
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Figure  23.  Azimuth-Elevation  Results  from  Newton-Euler  Simulation 

(Eull  Inertia  Tensor) 


The  az-el  configuration  with  full  inertia  tensor  was  then  replicated  in  the 
SimMechanics  workspace.  The  only  change  required  to  the  double  pendulum  system  was 
to  add  rotations  to  align  joint  one  to  the  y-axis  (-90°  rotation  in  the  x-axis  before  the  joint 
to  align  the  z-axis  to  the  y-axis,  and  a  90°  rotation  after  the  joint  to  realign  the  axes),  and 
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to  add  the  off-axis  inertia  components.  The  model  was  simulated  for  10  seconds, 
resulting  in  the  response  shown  in  Figure  24. 
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Figure  24.  Azimuth-Elevation  Results  in  SimMechanics  (Full  Inertia  Tensor) 


The  absolute  difference  between  the  MATLAB  and  SimMechanics  results  can  be 
seen  in  Figure  25.  As  in  the  case  of  the  double  pendulum,  minimal  differences  are 
observed  and  these  can  be  attributed  to  numerical  errors  in  the  ODE45  solver. 
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Figure  25.  Azimuth-Elevation  Simulation  Residuals 


D,  AZIMUTH-ELEVATION  SYSTEM  WITH  PD  CONTROLLER 

The  final  validation  and  verification  test  evolution  was  the  addition  of  a  control 
system  to  the  model  to  investigate  the  effects  of  a  dynamically  applied  control  torque.  A 
Proportional-Derivative  (PD)  controller  was  chosen  for  its  simplicity  and  ease  of 
insertion  into  the  model.  The  PD  control  system  only  requires  knowledge  inputs  for 
position  and  velocity.  Both  of  these  values  are  readily  available  in  both  the  MATLAB 
and  SimMechanics  model  as  the  joint  angle  and  angle  rate. 

In  a  simple  PD  controller,  the  current  system  position  and  velocity  are  compared 
to  a  desired  endpoint  position  and  velocity.  The  difference  between  current  and  desired 
values  creates  an  error  signal.  These  position  and  velocity  error  signals  are  then 
multiplied  by  a  proportional  gain  and  a  derivative  gain,  Kp  and  Kd,  respectively.  Finally, 
the  gained  signals  are  summed  and  the  result  becomes  the  control  torque  applied  to  the 
joint,  as  shown  in  Figure  26. 
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Figure  26.  PD  Controller 


The  shape  of  the  system  response  is  eontrolled  by  adjusting  the  proportional  and 
derivative  gain.  By  tuning  these  two  variables,  a  system  ean  be  driven  to  an  over-damped, 
eritieally  damped,  or  underdamped  response,  with  any  desired  settling  time,  natural 
frequeney,  overshoot,  ete.  For  the  simulation,  a  oritieally  damped  system  was  utilized, 
where  the  joint  would  rotate  to  the  desired  position  as  quiekly  as  possible  without 
overshooting  the  endpoint.  In  order  to  develop  this  response,  the  following  proeedure  was 
used  .  This  proeedure  is  not  intended  to  cover  the  derivation  or  the  full  explanation  of  a 
PD  controller.  Instead  it  is  meant  as  a  straightforward  shortcut  to  implementing  a 
rudimentary  PD  controller.  For  a  full  discussion  of  controller  derivations  and 
customization,  see  [20]. 


Starting  with  the  settling  time,  ,  of  20  seconds,  and  utilizing  a  damping  ratio, 
4” ,  of  unity  (necessary  for  a  critically  damped  system),  a  natural  frequency,  ,  was 
determined  by  utilizing  Equation  (5.8). 

(5-8) 
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4  4 

co= - =  —  =  0.2 

a  20 


(5.9) 


From  here,  the  required  gains  eould  be  solved  using  Equation  (5.10)  and  Equation 
(5.11),  where  /  is  the  effeetive  inertia  of  the  body  about  the  gimbal  axis 

Kp  =  Io)^^  =0.041  (5.10) 

Kd  =  21^0^=0.41  (5.11) 

Now,  a  gimbal  eontrol  torque  ean  be  generated  using  Equation  (5.12),  where  9^ 

is  the  desired  final  joint  angle,  and  9^  is  the  desired  final  joint  rate. 

T^^=Kp{9-9,)  +  Kd{9-9,)  (5.12) 


This  proeess  was  applied  to  both  the  azimuth  and  elevation  gimbal  using  the  same 
eonfiguration  as  the  full  inertia  az-el  model.  The  inertia  value  in  Equation  (5.10)  and 
Equation  (5.11)  were  set  to  the  sum  of  the  prineiple  inertia  value  of  eaeh  link  in  eaeh 
respeetive  axis  {lyy  for  azimuth,  4^  for  elevation),  and  adjusted  using  the  parallel  axis 
theorem  as  appropriate.  Gravity  was  disabled  in  order  to  investigate  the  effeets  of  the 
eontrol  logie  and  applied  torque  without  external  disturbanees.  The  desired  azimuth  angle 
was  set  to  45°,  and  the  desired  elevation  angle  was  set  to  15°.  The  desired  settling  point 
rates  for  both  azimuth  and  elevation  rates  were  set  to  zero.  This  setup  resulted  in  the 
response  shown  in  Eigure  27. 
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Figure  27.  Azimuth-Elevation  Results  from  Newton-Euler  Simulation 

(PD  controller) 


The  PD  controller  was  implemented  in  the  SimMechanics  model  using  additional 
blocks  in  the  joint  subsystems  as  shown  in  Eigure  28  and  Eigure  29.  Controller  gains 
were  solved  using  the  same  code  as  the  MATEAB  model. 
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Az  PD  Control 


Figure  28.  PD  Control  Feeds  in  Joint  Subsystem 


t1  u1 


Figure  29.  PD  Controller  Subsystem 

The  same  initial  eonditions  and  simulation  time  were  utilized  for  the 
SimMechanics  model.  This  resulted  in  the  response  depicted  in  Figure  30. 
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Figure  30.  Azimuth-Elevation  Results  from  SimMechanics  (PD  controller) 


Comparing  the  response  from  SimMechanics  to  the  response  given  by  the 
developed  code,  the  differences  were  negligible,  as  shown  in  Figure  31. 
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Figure  3 1 .  Azimuth-Elevation  PD  Controller  Difference 

E.  VALIDATION  AND  VERIFICATION  SUMMARY 

While  only  four  complete  validation  and  verification  tests  were  described  in  this 
chapter,  a  myriad  of  other  tests  were  run  in  order  to  thoroughly  vet  the  dynamics  model. 
These  included  unique  inertia  tensors,  various  system  dimensions  and  configurations,  and 
even  the  addition  of  external  forces  and  torques.  This  plethora  of  simulations  was 
essential  to  identification  and  resolution  of  errors  in  the  code  for  the  dynamics  model. 
Although  they  are  not  presented  here  due  to  the  number  of  simulations,  the  similarity 
between  each  simulation,  and  the  minutia  involved,  they  were  critical  to  ensuring  an 
accurate  dynamics  response. 

Most  importantly,  this  model  validated  the  row  addition  and  reduction  process 

described  in  Chapter  IV.  Tests  were  performed  where  the  rows  were  reduced  without  the 

proper  resultant  torque  propagation  and  the  results  whoed  that  the  torques  applied  to  the 

outer  bodies  did  not  influence  the  motion  of  inner  bodies.  Therefore,  without  the  addition, 
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or  some  other  method  of  solving  and  propagating  a  reaction  torque,  the  system  will  not 
respond  properly.  However,  for  each  case  investigated,  the  proposed  method  worked 
flawlessly,  and  was  able  to  produce  the  correct  result. 
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VI.  OPTIMAL  CONTROL  PROCESS 


A,  INTRODUCTION  TO  BOUNDARY  VALUE  PROBLEM 

One  approach  to  obtain  an  optimal  control  solution  is  to  formulate  a  boundary 
value  problem  (BVP).  The  BVP  is  a  very  powerful  tool  for  solving  optimal  control 
problems.  With  regard  to  this  thesis,  the  BVP  methodology  was  selected  to  explore  the 
solution  to  the  problem. 

To  solve  the  BVP,  Pontryagin’s  Principle  was  applied  [21].  This  process  begins 
with  the  definition  of  a  problem  statement.  The  scope  of  the  problem  is  reduced  by 
implementing  constraints  and  boundary  values.  The  problem  includes  a  control  function, 
u ,  and  a  state  space  model,  x  =  f  {x,u,t) ,  to  describe  the  system.  The  control  function  is 
the  input  to  the  state  model  that  drives  a  response.  This  problem  statement  is  used  to 
develop  and  minimize  a  function  called  the  Hamiltonian,  H.  Pontryagin’s  Principle  states 
that  the  optimal  control  function  minimizes  the  Hamiltonian  for  every  instant  in 
time  [21].  Therefore,  if  the  Hamiltonian  can  be  minimized,  the  control  that  drives  this 
minimization  will  be  the  optimal  solution  to  the  stated  problem.  To  begin,  a  problem 
statement  is  formulated  for  the  BVP.  This  consists  of  three  parts:  a  function  or  objective 
to  minimize,  the  dynamics  of  the  system,  and  a  set  of  boundary  conditions  and 
constraints. 

The  function  or  objective  to  minimize  is  the  cost  function,  J.  The  cost  function  is 
a  function  of  the  system  states  and  the  control  and  can  have  two  components,  an  endpoint 
cost  (Mayer  cost),  E,  and  a  running  cost  (Lagrange  cost),  F.  The  cost  can  consist  of  either 
or  both  of  these  two  components  [21].  The  endpoint  cost  is  a  function  of  the  states  at  the 
final  time,  and  possibly  the  final  time  itself.  Minimizing  an  endpoint  cost  will  minimize 
some  desired  value  at  the  final  time  of  the  problem.  For  example,  minimizing  the  final 
time  will  yield  the  time  optimal  solution.  Minimizing  values  such  as  position  or  velocity 
error  can  maximize  accuracy  by  minimizing  final  endpoint  error.  The  running  cost  is  a 
function  of  the  entire  state  trajectory,  the  control  function,  and  the  time.  The  running  cost 
is  calculated  over  the  entire  path  of  the  solution.  A  running  cost  can  be  used  to  minimize 
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energy,  eontrol  effort,  ete.  For  example,  by  using  spaeeeraft  body  angular  momentum  as 
a  running  eost,  an  optimal  solution  to  minimize  the  disturbanee  eaused  by  moving  the 
antenna  ean  be  found.  The  entire  eost  ean  be  represented  by  the  Bolza  eost  funetional, 
depleted  in  Equation  [21], 

J  =  E  +  ^Fdt  (6.1) 

^0 


The  dynamies  of  the  system  are  represented  by  the  dynamie  equations  developed 
in  Chapter  II  and  Chapter  III,  or  their  simplified  form.  The  system  is  deseribed  as  a  state 
spaee  model,  where  a  state  and  its  time  derivative  are  established.  For  the  general  three 
link  system,  the  dynamies  equations  would  be; 
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u  = 


(6.3) 


Lastly,  the  boundaries  and  eonstraints  of  the  problem  are  defined.  Eaeh  state  ean 
be  given  any  eombination  of  fixed  values,  boundary  eonditions,  or  funetions.  For 
example,  an  initial  position  and  veloeity  can  be  defined,  or  a  lower  and  upper  boundary 
can  be  defined  in  order  to  specify  a  range  of  initial  values  to  let  the  solution  of  the  dictate 
the  optimal  value.  Boundaries  can  be  placed  on  values  over  the  entire  path  of  the 
problem,  forcing  a  constraint  on  the  solution.  For  example,  lower  and  upper  angle 
constraints  could  be  set  to  model  soft  or  hard  stops  for  joints,  or  limits  could  be  set  on 
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control  torque  to  constrain  the  maximum  motor  output.  Lastly,  fixed  values  or  boundary 
conditions  could  also  be  funetions  of  (i.e.,  functions  of  time  or  other  states),  as  opposed 
to  explicit  quantities. 


Putting  all  these  components  together,  the  problem  statement  can  be  written  in  a 
eonvenient  form  [21]. 
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This  problem  setup  defines  initial  conditions  for  the  state,  initial  time,  final 
conditions  for  only  joint  orientation,  and  boundaries  for  joint  control  torque.  Other  final 
states  and  intermediate  states  (states  at  any  time  between  initial  and  final)  are  not 
bounded.  This  will  be  the  basic  foundational  problem  statement  for  all  subsequent 
optimal  control  solutions.  Slight  additions  or  modifications  will  be  made  on  a  case-by 
case  basis  to  fit  the  desired  model  or  objective.  Specifically  different  cost  functions  will 
be  implemented  along  with  various  other  constraints. 


With  the  problem  formulation  established,  the  process  of  formulating  the  BVP 
can  begin.  First,  the  adjoint  convector  is  introduced.  The  adjoint  convector  is  a  vector  of 
costates  (A) ,  where  every  costate  is  associated  with  a  state,  and  effectively  mirrors  the 
time  history  of  the  state  in  a  non-linear  way  [21].  The  costates  essentially  provide  a 
reference  for  the  cost  of  each  state  that  is  not  obstructed  by  the  relative  magnitudes  of  the 
states  themselves  [21]. 


Using  this  costate  vector,  the  Hamiltonian  is  defined: 


(6.5) 


H  =  F  +  l^f  . 


(6.6) 


The  Hamiltonian  must  be  minimized  with  respect  to  control,  and  the  control 
trajectory  that  achieves  this  result  is  considered  the  optimal  control  trajectory. 


The  solution  is  obtained  by  with  the  Hamiltonian  Minimization  Condition,  which 
in  the  absence  of  constraints  on  the  controls  is  also  known  as  the  Euler-Lagrange 
equation. 


dH 

du 


=  0 


(6.7) 
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However,  the  application  of  the  Hamiltonian  Minimization  Condition  depends  on 
knowledge  of  the  dynamics  of  the  costates.  Therefore,  the  costates  must  be  solved  first 
using  a  series  of  differential  equations. 


Each  costate  is  defined  by  an  adjoint  equation,  expressed  in 
Equation  (6.8)  [21]. 

dH 


-X  =  - 


dx 


(6.8) 


The  costate  vector  and  the  state  dynamics  each  comprise  systems  of  differential 
equations  that  must  be  solved  to  express  the  Hamiltonian  so  that  the  Hamiltonian 
Minimization  Condition  can  be  applied.  The  introduction  of  the  adjoint  equation  results 
in  a  system  of  2N  variables  {N  states  and  N  costates),  and  2N  differential  equations, 
requiring  2N  known  point  conditions  to  generate  a  unique  solution  [21].  Eor  the  three 
body  problem  statement  in  Equation  (6.4),  there  are  25  unique  states.  The  problem 
statement  includes  25  unique  boundary  conditions  at  the  initial  time,  but  only  six  unique 
boundary  conditions  at  the  final  time.  Therefore,  there  are  50  differential  equations  with 
31  boundary  conditions  specified.  If  every  state  and  costate  were  provided  with  an 
endpoint  condition,  there  would  be  50  equations  with  50  boundary  conditions,  and  a 
solution  could  be  found.  It  is  also  important  to  note  that  if  final  time  is  not  explicitly 
defined,  this  results  in  an  additional  boundary  condition,  resulting  in  a  total  of  32 
necessary  conditions  for  the  general  problem  statement. 


In  order  to  solve  for  the  additional  boundary  conditions,  the  Endpoint  Eagrangian, 
E ,  the  vector  of  Eagrange  multipliers,  v,  and  the  terminal  Transversality  Condition  are 
introduced  [21].  Eirst,  the  vector  of  Eagrange  multipliers  contains  one  variable  for  every 
state  that  has  an  explicit  endpoint  condition.  Eor  example,  the  general  three  body  problem 
statement  provides  two  endpoint  conditions;  therefore  there  are  two  associated  Eagrange 
multipliers. 

(6.9) 

This  vector  is  used  in  the  definition  of  the  Endpoint  Eagrangian,  where  e  is  a 
vector  of  state  endpoint  conditions  [21]. 


81 


E  =  E  +  v^e 


(6.10) 


Lastly,  the  terminal  Trans versality  Condition  is  introdueed  [21], 


dE 


(6.11) 


Together,  Equation  (6.10)  and  Equation  (6.11)  provide  the  missing  conditions. 
Eor  example,  application  of  the  Transversality  Condition  introduces  an  additional  24 
equations,  but  only  introduces  6  additional  variables.  This  results  in  producing  18  of  the 
missing  conditions  since  many  of  the  partial  derivatives  are  zero.  If  every  state  were 
provided  with  an  endpoint  condition,  N  additional  Eagrange  multipliers  would  be 
introduced  along  with  N  additional  differential  equations.  Therefore,  since  the  problem 
would  already  have  2N  point  conditions  before  calculating  terminal  Transversality,  this 
step  becomes  superfluous  and  no  additional  information  would  be  obtained. 


One  missing  condition  remains  since  final  time  was  not  defined.  The 
Hamiltonian  Value  Condition  is  therefore  introduced  to  satisfy  the  final  necessary  point 
condition,  as  defined  in  Equation  (6.12)  [21].  This  step  is  also  superfluous  if  a  final  time 
is  specified. 


Hitf)  =  - 


dE 

dtf 


(6.12) 


It  can  be  noted  that  if  there  are  constraints  to  the  problem,  particularly  with 
respect  to  control  constraints,  additional  terms  are  required  in  the  Hamiltonian  and  the 
Euler-Eagrange  equation  no  longer  applies.  Instead,  the  partial  derivative,  5H/0u  must  be 
interpreted  as  a  switching  function. 


This  overview  of  optimal  control  theory  is  meant  to  introduce  the  reader  to  some 
of  the  comcepts  involved,  but  is  not  meant  to  be  an  exhaustive  treatment  Eor  a  more 
thorough  discussion,  see  [14]  and  [21]. 


B,  DOUBLE  PENDULUM  EXAMPLE 

Most  optimal  control  problems  quickly  become  so  complex  that  they  require 
numerical  techniques  to  solve.  Nonetheless,  the  double  pendulum  is  an  example  of  a 
system  that  is  simple  enough  to  provide  a  useful  example  of  the  steps  involved  in  this 
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process.  To  begin,  the  same  double  pendulum  system  as  used  previously  in  validation  and 
verifieation  ean  be  modified  to  provide  an  example  applieation  of  the  approaeh. 
Eliminating  gravity  further  reduees  the  eomplexity,  and  assuming  the  availability  of  a 
eontrol  torque  for  eaeh  gimbal  provides  a  eontrolled  system  to  optimize.  The  dimensions, 
masses,  and  inertias  of  the  system  were  maintained  from  the  validation  and  verification 
model. 

After  performing  system  tailoring  and  solving  for  the  dynamie  equations  of  the 
system,  the  state  spaee  in  Equation  (6.13)  is  produeed. 

x=r> 


x  =  \ 


-67)jj  +67^2 -^Q0^sin{02)-2>^02sin{02)+9TQ^cos{02)-(^0^02sin{62)-A50^cos{02)sin{0^ 

i45cosi0,f -SO) 

(127)jj  -  607^2  +  ^OO0^^sm{02 ) + 6O02sin(02 ) + 9O0^sin{202 ) + 4502  sin{202 )+••• 
..AST^2fos{02)  -36Tq2Cos{02) +l2O0^02sm{02) +9O0^02sm{202)) 


(45oos(2^2)  - 115) 

4 


(6.13) 


u  = 


Suppose  the  goal  of  the  system  is  to  point  the  outer  pendulum  link  at  some  angle 
with  respect  to  the  inertial  frame,  and  suppose  the  desired  optimization  objeetive  is  a 
eombination  of  the  minimum  time  with  the  minimum  amount  of  torque.  This  gives  the 
cost  function  represented  in  Equation  (6.14). 

J  =  +  +  (6.14) 
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The  point  constraints  of  the  problem  are  then  introduced.  Suppose  the  starting 
joint  angles  and  rates  are  set  to  zero,  the  initial  time  is  zero,  the  final  rates  are  zero,  and 
the  final  combined  angle  is  90°  (any  combination  of  joint  one  and  joint  two  angles  that 
will  result  in  the  outer  link  pointing  at  90°  relative  to  the  inertial). 

Lastly,  some  control  and  state  boundaries  are  defined,  effectively  limiting  the 
solution  space  available  to  the  solver.  The  minimum  and  maximum  torque  limit  for  both 
joints  is  set  to  -1  Nm  and  1  Nm,  respectively.  Joint  angles  are  allowed  to  move  freely 
between  -180°  and  180°.  Finally,  the  joint  rates  are  constrained  to  between  -5  °/s  and 
5  °/s. 

This  results  in  a  relatively  straightforward  problem  definition. 


6*  = 


x  =  [e,  4  ^2} 

Minimize  J  =  1^^ —  |  (Tqi^ + 

^‘0 

Subject  to: 

-67^j  +  6Tq2  -  2)06^sin{62 )  -  3O02^sin{02 ) + 9Tq2Cos{62 )  -  6044‘^^^(^2 )  “  ^36^cos{62  )sin{02 ) 

(45my(47^80) 

(127jjj  -  607jj2  +  3OO0^^sin{02 ) + (^02  ^in{d2 ) + 900^ sin{2d2 ) + ^502  sin{202 ) + . . . 

^  _  ...\'SrQfos{02)-36TQ2Cos{02)+V190j)2sin{02)+990^02sin{202j) 

(45cos(2^2)-115) 


(4(g,4(to),^,(g,^2(4))=(o,o,o,o) 

(4(t,),4(t,),^,(t,))=(o,o,|-^2(^/)) 

Figure  32.  Double  Pendulum  Optimization  Problem  Definition 

Notice  that  since  it  is  irrelevant  how  the  final  angle  is  apportioned  between  the 
two  joint  angles,  a  single  endpoint  for  joint  one  was  defined  in  terms  of  the  final  total 
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angle  and  final  endpoint  for  joint  two.  This  ehoiee  is  completely  arbitrary,  but  is 
necessary  in  order  to  set  a  final  boundary  condition. 


Starting  the  solution  process,  the  Hamiltonian  is  first  calculated. 

H  =  F  +  X'<c 

H  =  -{Tq^  +Tq^)  +  4  +  4  +  Xg^  4 

The  Hamiltonian  Minimization  Condition  is  applied  next  to  give; 

H  =  |(To,"+  To/)  +  A,  9,  +  +  A,_4 

+X  { - ittk - )+A 

aTgj  ^\A5cos{e^f-%Q)  (45cos(2^2)-115) 

67^2+97^2£^5^  60T^2-^6T^2Cos(^2)._q 

dT^,  (45co5(^2)"-80)  ^  (45cos(2^2)-115)  ^ 

Solving  for  the  controls  yields; 

Ta,=-l  +  Xg  ( - - )  -  Xg 

°  ^'  (45005(^2)'- 80)  (45cos(2^2)-115) 

T  --1-2  /  6  +  9co5(6>2)  ,  60-36co5(6>2) 

^‘\45cos(02f-8Oy  '^^X45cos(2^2)- 115)^’ 

Next,  the  adjoint  equations  are  calculated. 


(6.15) 


(6.16) 


(6.17) 
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dH 
^  du, 


...  +  - 


i  -M- 

du^ 


...  +  - 


Xg  {-(X)9^sin{9j)  -  ^W^sinifij)  -  9W^cos{9j)sin{9jX^ 

(45cos(^7-80)  ' 

Xg  {600^^sin{^2)  +  lS0^^sin{2^2)  +  12045/n(^2)  +  ^QO^sinilO^)) 
(45cos(26'2)-  115) 

Xg  (-6O^2‘^/«(02)  ~  ^QO^siniOj)) 

{AScosiO^f ■■■ 

Xg  (12045/n(^2)  +  9O4‘Sm(202)  +  12O0,5/n(^2)  +  ^^OySiniXlO^') 


+  Xn 


(45cos(26'2)-  115) 


+  Xn 


dO, 


;  dH 

^9.  ~  -  ~ 


de, 


...  +  - 


Xg  {-'iWy  cosiOj)  -  IW^cosiOj)  -  A59^  cos^Oj)  +  A59^  sin{92)  -  97’g25/«(^2)  “  6O0j02‘^o5(^2)) 

(45co5(6'2)"-80)  ■■■ 

Xg  (9Ocos(02) si'i(^2))(“6rgj  +  67)j2  -  'iQ9^sin{92)  -  3O4^5/n(02)  +  97);2CO5(02)  “  6O0j^2‘^/«(02)  ^  45^,^co5(02)‘^^”(^2)) 


...+ 


(45cos(02)"-8O)" 

1^(300^"  cos(6'2)  +  cos(6'2)  +  180^"  cos(202)  +  904"  cos(26'2)... 

...  -  187)jj  sin(02)  +  36rg2  sin(02)  +  12O0j4  008(^2)  +  180^j4  008(2^2)) 

■■■^  (45co8(202)-  115)  ■■■ 

Xg^  (908in(26'2))(12rg;  -  60rg2  +  2QQ9^  sin{92)  +  604"5/«(6»2)  +  9Q9^hin{292)  +  454"5/«(26'2)... 

...187);jC05(^2)  ~  26Tq2Cos{92)  +  12O^,025/«(^2)  +  9O^j4‘^m(202)) 

(45008(26*2)  -  115)" 


(6.18) 


86 


At  this  point,  the  problem  contains  eight  differential  equations,  but  only  seven 
point  conditions  (four  point  conditions  for  initial  conditions,  and  three  defined  final 
conditions).  This  leaves  two  additional  endpoint  conditions,  including  one  required  since 
final  time  is  not  specified.  Therefore,  the  Terminal  Transversality  and  the  Hamiltonian 
Value  Condition  are  both  necessary  steps. 

First,  the  Endpoint  Lagrangian  is  calculated; 

E  =  E  +  v^e  =  tf+v^{e,f)  +  v^{e^f)  +  v^{^-e^f)  (6.19) 

Using  this.  Terminal  Transversality  is  applied  to  yield  four  additional  equations, 
while  only  adding  three  additional  variables. 


f^F 

4(U)  =  ^^  =  U 

'  m,  ) 

E  (/  )  =  ^^  =  V2 

'  mj  ) 

f)F 

=  =  0 


BE 


Xn  (tf)  =  -  =  -V, 

^  e(^2/) 


(6.20) 


Lastly,  the  Hamiltonian  Value  Condition  yields  the  final  condition  necessary  for  a 
unique  solution. 


=  =  -l 


(6.21) 


ft  can  be  seen  that  the  equations  yielded  by  this  process  can  become  difficult  to 
solve,  even  for  a  simple  problem  like  the  double  pendulum.  To  solve  the  BVP,  various 
methods  can  be  utilized,  such  as  a  shooting  or  collocation  technique.  Shooting  uses  a 
guess  and  check  method  to  hone  in  on  the  optimal  solution.  However,  shooting  suffers 
from  the  curse  of  sensitivity  [21],  in  which  the  Hamiltonian  BVP  has  poles  on  both 
halves  of  the  imaginary  plane  which  causes  issues  in  propagating  the  equations. 


To  circumvent  the  curse  of  sensitivity,  collocation  can  be  used.  Using  this 


approach,  the  timeframe  is  split  into  discrete  time  intervals  and  the  entire  solution  is 
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obtained  at  once  using  linear  algebra  of  the  form  Ax=b.  However,  this  type  of  solution 

suffers  from  the  curse  of  dimensionality,  in  that  the  size  of  the  discretized  system  grows 

2 

exponentially  as  smaller  time  intervals  are  utilized.  A  system  of  A  intervals  requires  A 
elements.  For  example,  if  a  system  is  split  into  10  intervals,  a  square  matrix  solution  of 
100  elements  is  required.  For  systems  with  quick  dymanics,  more  time  divisions  are 
necessary.  This  can  at  best  slow  down  the  solver,  and  at  worst  break  the  solver.  This 
problem  has,  however,  been  alleviated  over  the  last  decades  with  the  implementation  of 
more  complex  and  more  powerful  computing  platforms.  One  additional  way  to  help 
improve  solution  efficiency  and  accuracy  is  to  first  solve  the  system  using  a  coarse  grid, 
and  then  use  this  solution  as  a  guess  and  solve  the  system  again  on  a  finer  grid.  Building 
up  a  solution  in  this  way,  an  accurate  solution  can  be  obtained  faster  without  the  need  for 
extensive  computational  hardware. 

C.  SOLVING  THE  OPTIMAL  CONTROL  PROBLEM  IN  DIDO 

Even  though  the  equations  to  solve  the  BVP  for  optimal  control  of  the  double 
pendulum  model  can  be  derived,  they  are  quite  complex.  Thus,  the  solution  must  be 
obtained  using  an  optimal  control  software  package.  One  such  optimal  control  program  is 
DIDO  [14].  DIDO  is  a  MATLAB  based  software  suite  that  is  specifically  designed  to 
easily  and  quickly  solve  optimal  control  problems  using  the  theory  described  in  Section 
A.  DIDO  only  requires  the  dynamics  equations  and  the  bounds  on  the  system,  then  solves 
the  optimal  control  problem  using  the  Legendre  pseudospectral  method  [22].  Most 
problem  definitions  are  easy  to  implement  in  DIDO  and  the  software  allows  for  easy 
variation  of  the  problem  in  order  to  explore  a  wide  range  of  problem  definitions. 
Furthermore,  DIDO  provides  tools  to  allow  insight  into  controls,  states,  covectors,  etc.,  in 
order  to  glean  deeper  understanding  of  a  system  response  or  to  troubleshoot  the  problem 
specification. 

To  begin,  the  user  must  define  the  problem.  This  involves  specifying  five 
MATLAB  files:  a  dynamics  file,  a  cost  file,  an  events  file,  an  optional  path  file,  and  a 
problem  file.  The  dynamics  file  describes  the  state  dynamics  of  the  system.  The  states  are 
provided  for  each  interval  in  time  and  the  dynamics  are  returned  for  each  instant.  The 
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cost  file  describes  the  cost  function  of  the  system  as  both  an  endpoint  cost  and  a  running 
cost.  The  events  file  describes  the  known  boundary  conditions  on  the  system  states.  The 
path  funetion  provides  additional  constraints  on  the  solution  at  each  node,  as  required. 
Lastly,  the  problem  file  deseribes  the  scales  and  boundaries  of  the  problem,  and 
coordinates  the  solution.  For  example,  the  problem  file  establishes  how  many  states  and 
what  state  variables  make  up  the  problem,  as  well  as  what  boundaries  are  specified  for 
each  state  variable,  limiting  the  solution  space.  The  problem  file  also  establishes  the 
values  necessary  for  the  event  conditions,  i.e.,  the  initial  and  final  values  of  each  state. 

Once  these  MATLAB  files  are  provided,  DIDO  automatically  solves  the 
conditions  necessary  for  Pontryagin’s  Principle  and  finds  an  extremal  solution  by 
applying  the  equations  described  in  Section  A.  The  solution  for  a  version  of  the  double 
pendulum  problem  with  state  and  control  constraints  can  be  seen  in  Figure  33  and 
Figure  34.  In  the  solution  shown,  the  gimbal  rates  and  gimbal  torques  were  constrained  to 
be  less  than  5°/s  and  0.25  Nm,  respectively. 
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Gimbal  1  Angle 


Gimbal  2 


Figure  33.  Optimal  Control  of  the  Double  Pendulum  (State  Trajectories) 

In  order  to  ensure  the  solution  provided  by  DIDO  is  feasible,  the  dynamics  of  the 
system  are  driven  by  the  controls  solved  by  DIDO.  This  propagation  can  be  done  using 
ODE45.  The  results  of  this  validation  and  verification  test  are  seen  as  the  solid  colored 
line  in  Figure  33,  whereas  the  DIDO  trajectory  is  gived  with  the  dotted  black  line.  With 
the  approach  to  optimal  control  outlined  and  validated,  the  DIDO  program  was  utilized  to 
investigate  problems  relevant  to  slew  control  of  satellite  antennas. 
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Gimbal  1  Torque 


Gimbal  2  Torque 


CD 

CT 


Figure  34.  Optimal  Control  of  the  Double  Pendulum  (Control  Trajectories) 
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VII.  TDRS  OPTIMIZATION 


A,  TDRS  SLEW  CONTROL 

The  primary  goal  of  this  thesis  is  to  explore  the  optimization  of  TDRS  antenna 
slews.  However,  in  order  to  properly  formulate  the  problem  statement  and  in  order  to 
compare  optimum  slew  improvements  over  current  slews,  the  conventional  approach  to 
controlling  the  TDRS  SA  antenna  must  be  understood.  Current  slew  control  of  the  SA 
antenna  is  described  in  this  section. 

A  slew  and  track  consists  of  three  parts,  an  open  loop  program  track  in  order  to 
move  the  antenna  from  one  user  to  another,  a  pull  in  phase,  and  an  autotrack  phase  [1]. 
The  program  track  gets  the  antenna  pointing  close  enough  that  a  signal  feedback  loop  can 
be  implemented  so  that  the  pull  in  phase  can  reduce  the  error  from  around  0.22°  to  0.03°. 
Next,  the  system  enters  the  auto  track  phase  where  it  will  remain  for  the  extend  of  the 
relay  [11,  10].  Once  in  auto  track,  the  beamwidth  of  the  communications  signal  drives 
stringent  pointing  requirements,  as  shown  in  Table  2.  Extensive  study  has  been  given  to 
improving  the  closed  loop  pull  in  and  autotrack  phases  of  the  slew  in  order  to  achieve 
these  high  accuracies.  However,  minimal  work  has  been  done  on  the  open  loop  program 
track  phase.  Furthermore,  no  evidence  was  found  of  any  optimization  efforts  for  this 
portion  of  the  maneuver.  Therefore,  this  chapter  will  focus  on  the  modeling  and 
optimization  of  large  program  track  maneuvers. 


Field  of  Mew  (degrees) 

Maximum  Antenna  Pointing  Error  (degrees) 

E  \V  (AZ) 

N-S  (EL) 

SSAF 

SSAR 

KuSAF 

KuSAR 

KaSAF 

KaSAR 

PT,  LEO 

+/-10.5“  (’onkal 

— 

0.105“ 

0.102“ 

PT  PEFOV 

+/-22.5‘’ 

+/-31.0“ 

0.360° 

0.360° 

0.155° 

0.155° 

0.114° 

0.114° 

AT  PEFOV 

1  /-22.5“ 

i/-31.0° 

0.087° 

0.061° 

0.073° 

0.045° 

Nominal  .Antenna  Beamwidth 

~2.1” 

-2.0° 

-0.31° 

-0.28° 

-0.18° 

-0.17° 

PEFOV 

Primary  Elliptical  Field  of  View 

Table  2.  LEO  and  Primary  FOV  Pointing  for  TDRS  H,I,J  (from  [6]) 
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Six  minutes  prior  to  service  start  time,  WSC  computes  the  slew  maneuver  and 
uploads  it  to  the  satellite  as  coefficients  for  a  third-order  polynomial  to  control  gimbal 
position  [1,  8,  10].  TDRS  uses  a  gimbal  drive  assembly  to  follow  the  desired 
polynomial  [10].  While  the  program  track  is  considered  an  open  loop  input,  it  does  use  a 
resolver  signal  to  provide  position  feedback  [1].  A  rate  command  derived  from  the 
position  profile  is  directly  applied  to  the  stepper  motor  and  an  estimated  torque  command 
is  fed  forward  to  the  system  as  well  [1].  The  gimbal  drive  assembly  uses  a  two-phase 
permanent  magnetic  stepper  motor  [10].  The  motor  has  a  1.5°  step  size,  which  is  stepped 
down  by  a  harmonic  drive  speed  reducer  with  a  reduction  ratio  of  200:1  [10].  This  yields 
a  resolution  of  0.0075°,  small  enough  to  ensure  the  minimum  0.03°  pointing  is  met 
during  autotrack.  The  program  track  slew  consists  of  three  segments:  a  five  second 
acceleration,  a  constant  rate  coast,  and  a  five  second  deceleration  [10].  For  maneuvers 
where  one  slew  axis  is  larger  than  the  other,  the  constant  slew  rate  is  adjusted  so  that  the 
slews  end  concurrently  [11]. 

B,  TDRS  MODEL 

A  general  multibody  model  was  developed  in  order  to  simulate  the  dynamics  of 
TDRS  and  SA  antenna  during  slew  maneuvers.  However,  this  process  requires  masses, 
inertias,  and  geometric  dimensions  in  order  to  properly  model  the  spacecraft. 
Unfortunately,  even  though  effort  was  made  to  obtain  the  necessary  data  from  Boeing, 
the  release  of  this  data  was  not  approved  in  time  for  this  thesis.  Nevertheless, 
assumptions  were  made  based  on  open  source  TDRS  data,  TDRS  drawings,  and  scaled 
representations  from  a  laboratory  testbed  [23]. 

The  total  mass  of  TDRS-K  is  3,454  kg  at  launch,  including  fuel  [24].  An 
estimated  operation  mass  of  3,310  was  assumed  for  fuel  expenditure  for  geostationary 
orbit  raising  and  circularization.  This  was  allocated  as  3,200  kg  for  the  spacecraft  body, 
including  bus,  solar  panels,  Space-Ground  Link  Antenna,  forward  and  rear  omni  TT&C 
antennas,  and  the  Western  SA  antenna,  and  Eastern  SA  antenna  deployment  boom.  These 
systems  were  combined  into  one  rigid  body  and  considered  body  one  of  the  system.  The 
gimbal  assembly  from  azimuth  gimbal  to  elevation  gimbal  was  allocated  10  kg  and 
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considered  body  two.  Lastly,  the  SA  antenna  itself,  including  the  payload  electronics  and 
wiring  in  the  SA  compartment  (SAC)  were  allocated  100  kg  and  considered  body  3.  A 
coordinate  system  was  assigned  in  order  to  align  the  gimbal  axes  of  TDRS  with  the 
gimbal  axes  of  the  general  model.  The  x-axis  was  aligned  with  nadir,  the  y-axis  was 
aligned  with  the  North  vector,  and  the  z-axis  was  aligned  to  the  positive  velocity  or  East 
vector.  This  configuration  can  be  seen  in  Figure  35. 


Figure  35.  TDRS  Model  Configuration  (after  [4]) 

Inertia  estimates  for  the  body  were  made  by  assuming  a  constant  density  of  the 
bus  and  calculating  the  moments  of  inertia  of  a  3x3  meter  box.  Mass  was  then  distributed 
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unevenly  to  aecount  for  the  solar  arrays,  Western  SA  antenna,  Space-Ground  Link 
antenna.  Then  the  inertia  was  shifted  slightly  toward  the  Western  SA  antenna  in  order  to 
account  for  CoM  of  the  amalgamated  system.  This  resulted  in  the  following  inertia  matrix 
estimate; 


5000  -50  -250 

-50  5000  -100 

-250  -100  4800 


kgrn^ 


The  distance  from  the  CoM  of  body  one  to  the  azimuth  gimbal  (joint  one)  was 
estimated. 


0.5 

0.1 

5.0 


>m 


The  azimuth  gimbal  was  aligned  to  the  positive  y-axis.  Any  outboard  (East) 
angles  measure  negative  and  inboard  angles  (West)  measure  positive.  The  distance  from 
the  CoM  of  body  two  to  the  azimuth  gimbal  was  estimated. 


-0.1 

0.0 

-0.5 


>m 


Body  two,  which  simulates  the  gimbal  assembly  system  was  estimated  as  a 
1  meter  by  .25  meter  box,  and  small  off  axis  inertia  components  added  as  necessary.  The 
relative  size  and  mass  of  this  body  are  negligible  compared  to  bodies  one  and  three,  and 
the  system  could  arguably  be  modeled  as  a  two-body  system  with  a  two-axis  gimbal. 
However,  this  system  was  maintained  in  a  three-body  configuration  to  increase  accuracy 
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and  align  the  system  with  the  validation  and  verification  work  already  performed  on  a 
three-link  azimuth-elevation  system. 
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The  distance  between  the  CoM  of  body  two  and  the  elevation  gimbal  (joint  two) 
were  estimated,  as  well  as  the  distance  between  the  CoM  of  body  three  and  the  elevation 
gimbal. 
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Lastly,  the  inertia  of  the  SA  antenna  was  estimated.  This  started  by  scaling  the 
mass  and  dimension  of  the  testbed  antenna,  then  adding  mass  as  necessary  to  simulate  the 
SAC  and  the  launch  support  structure.  During  launch,  a  truss  assembly  connects  the 
stowed  SA  antenna  to  the  body  in  order  to  alleviate  launch  stresses.  During  antenna 
deployment,  this  structure  is  separated  from  the  spacecraft  body  and  remains  connected 
to  the  SAC.  This  process  resulted  in  the  inertia  estimate  in  Figure  42. 


97 


^3  = 


180.0 

8.0 

10.0 

8.0 

205.0 

0.5 

10.0 

0.5 

215.0 

kgm^ 


This  data  was  input  into  the  general  model  in  order  to  generate  a  TDRS  dynamics 
model.  However,  more  assumptions  were  made  to  the  model  as  a  whole  to  simplify  the 
scope  and  complexity  of  the  problem  to  this  thesis  as  a  feasibility  study.  The  validation 
and  verification  steps  taken  in  Section  B  were  vital  for  justifying  these  assumptions. 

First,  the  gimbal  angles  about  each  joint  were  considered  the  azimuth  and 
elevation  pointing  angles  necessary  to  point  the  antenna  at  the  user  spacecraft.  However, 
as  the  spacecraft  body  rotates  during  a  slew,  the  spacecraft  angle  changes  the  necessary 
pointing  angles.  On  TDRS,  the  spacecraft  body  rotation  and  attitude  determination  and 
control  system  (ADCS)  coordinates  with  the  SA  controller  to  account  for  this  deviation. 
In  order  to  scope  the  problem,  the  ADCS  was  not  modeled.  Instead,  the  spacecraft  is 
assumed  to  maintain  nadir  pointing  with  enough  accuracy  to  nullify  the  SA  error.  In  order 
to  simulate  this,  a  maneuver  was  performed  using  the  maximum  gimbal  rate  (0.225°/s) 
and  the  spacecraft  allowed  to  drift.  Maximum  spacecraft  rates  were  measured.  These 
rates  were  taken  as  limits  in  the  antenna  slew  optimization.  It  is  assumed  that  if  the 
optimal  control  maneuver  can  adhere  to  these  constraints,  then  the  TDRS  control  system 
can  minimize  the  rotational  impact  of  the  maneuver  to  an  acceptable  margin  in  the  same 
way  as  is  done  for  a  conventional  slew. 

Next,  the  general  model  to  this  point  has  assumed  a  body  one  frame  rotating 
directly  about  the  inertial  axis.  In  other  words,  if  the  SA  antenna  is  stationary,  there  is  no 
rotation  about  body  one  relative  to  the  inertial  axis.  This  essentially  indicates  an  inertial 
axis  aligned  to  the  orbital  frame.  However,  the  orbital  frame  is  rotating  about  the  Earth  at 
360°  per  sidereal  day.  The  Earth  is  subsequently  orbiting  around  the  sun,  which  is 
moving  about  space,  etc.  Coupled  motion  is  induced  by  each  subsequent  rotation  about 
an  ideal  true  inertial  frame.  However,  as  scope  is  increased  from  body  frame  to  orbital 
frame,  to  Earth  fixed  frame,  etc.,  the  impact  of  each  rotation  becomes  smaller  and  smaller 
to  a  point  where  the  effects  are  minimal.  Eor  most  Earth  orbiting  satellites,  this  point  is 


98 


considered  the  Earth  Centered  Inertial  frame.  A  frame  is  fixed  to  the  center  of  the  earth 
and  aligned  to  the  First  Point  of  Aries,  or  the  Vernal  Equinox.  If  this  is  chosen  as  the 
inertial  frame,  the  orbital  frame  rotates  about  it  at  a  rate  of  0.0042  °/s.  In  effect,  this  adds 
one  more  rotation  in  order  to  go  from  the  inertial  frame  to  the  orbital  frame  to  the  body 
frame.  However,  since  this  rotation  is  small  relative  to  the  timeframe  of  the  desired  slews, 
it  was  assumed  that  the  impact  of  this  rotation  was  negligible  and  assumed  zero. 

As  described  in  Chapter  I,  the  flexible  modes  of  the  spacecraft  and  fuel  sloshing 
was  not  modeled.  It  was  also  determined  that  friction  in  the  gimbal  could  be  ignored  for 
the  scope  of  this  thesis.  Since  gimbal  torque  limits  were  unknown,  estimated  maximums 
and  minimums  had  to  be  developed.  In  order  to  do  this,  the  acceleration  of  the  antenna 
was  analyzed.  Accelerations  from  zero  to  max  gimbal  rate  were  observed  to  vary  from 
approximately  1.5  seconds  to  5  seconds  depending  on  the  source  [10,  11].  The 
differences  between  these  two  estimates  may  be  due  to  different  generation  of  spacecraft, 
or  scope  of  the  explanation  (i.e.,  the  five  second  acceleration  time  may  include  other 
delays).  In  order  to  maintain  conservatism,  a  five  second  acceleration  time  to  maximum 
rate  was  assumed.  The  model  was  generated  and  torque  varied  in  order  to  gain  insight 
into  the  response.  It  was  determined  that  approximately  0.2  Nm  of  torque  resulted  in  the 
proper  five  second  acceleration  from  0°/s  to  0.225°/s.  Friction  torque  was  then  ealeulated 
based  on  a  viscous  friction  coefficient  of  5e'^  Nms/rad  [10].  Using  this  frietion  coefficient 
and  the  peak  gimbal  rate,  a  frietion  torque  of  1.96e'^  Nm  was  ealeulated.  Although  during 
optimization,  the  gimbal  rate  was  allowed  to  reach  beyond  0.225°/s,  the  resulting  frietion 
torque  was  always  two  or  more  orders  of  magnitude  less  than  the  gimbal  torque. 
Therefore,  the  frietion  was  assumed  negligible  for  the  scope  of  this  thesis. 

C.  TDRS  MODEL  VALIDATION  AND  VERIFICATION 

In  order  to  ensure  basic  functionality  and  accuracy  of  the  TDRS  model,  as  well  as 
to  justify  the  assumptions  in  Seetion  B,  validation  and  verification  was  performed  on  the 
Newton-Euler  simulation  using  the  SimMeehanics  model. 

Three  tests  were  run  to  ensure  functionality  and  accuracy  of  the  dynamics.  First, 
an  initial  rate  of  0.225  was  induced  in  each  gimbal  and  the  system  allowed  to  drift  for 

99 


120  seconds.  The  gimbal  responses,  body  rotation  rates,  and  body  rotation  angles  were 
compared  between  both  models. 


Figure  36.  TDRS  Newton-Euler  Simulation  Spacecraft  Body  Rates  for  V&V  Test  1 
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Figure  37.  TDRS  Newton-Euler  Spacecraft  Body  Angles  for  V&V  Test  1 
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Figure  38.  TDRS  Newton-Euler  Gimbal  Response  for  V&V  Test  1 

The  same  system  was  then  modeled  in  SimMechanics,  and  used  to  verify  the 
MATLAB  response. 
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Figure  39.  TDRS  SimMechanics  Spacecraft  Body  Rates  for  V&V  Test  1 
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Figure  40.  TDRS  SimMechanics  Spacecraft  Body  Angles  for  V&V  Test  1 
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Figure  41.  TDRS  SimMechanics  Gimbal  Response  for  V&V  Test  1 

While  there  are  slight  deviations  of  the  response,  namely  with  respect  to  elevation 
gimbal  response,  the  deviations  are  on  the  magnitude  of  le'"^  for  angle  and  rate.  Since 
these  responses  will  be  dynamically  driven  and  maintained  by  the  torquers  for  the 
optimal  simulations,  these  errors  were  considered  negligible.  The  more  important 
response  was  the  spacecraft  body  angular  rate,  which  matched  very  closely.  This  was 
vital  to  the  assumption  regarding  the  TDRS  ADCS. 

In  a  second  test,  a  constant  torque  of  0.1  Nm  was  applied  to  each  gimbal  from  the 
rest  position  and  the  system  response  measured  for  20  seconds.  The  response  can  be  seen 
in  Figure  42  through  Figure  44. 
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Figure  42.  TDRS  Newton-Euler  Spacecraft  Body  Rate  for  V&V  Test  2 
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Figure  43.  TDRS  Newton-Euler  Spacecraft  Body  angle  for  V&V  Test  2 
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Figure  44.  TDRS  Newton-Euler  Gimbal  Response  for  V&V  Test  2 
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Figure  45.  TDRS  SimMechanics  Spacecraft  Body  Rate  for  V&V  Test  2 
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Figure  46.  TDRS  SimMechanics  Spacecraft  Body  Angle  for  V&V  Test  2 
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Figure  47.  TDRS  SimMechanics  Gimbal  Response  for  V&V  Test  2 

As  expeeted,  under  driven  eonditions,  the  responses  eorrespond  mueh  better  than 
under  free  rotation. 

A  third  validation  and  verifieation  test  performed  to  ensure  dynamie  aeeuraey  was 
a  simulated  antenna  slew.  This  test  was  also  utilized  to  obtain  bounds  on  the  disturbanee 
induced  on  the  spacecraft  body  for  a  standard  antenna  slew.  A  0.2  Nm  torque  was  applied 
to  a  resting  system  for  five  seconds.  At  five  seconds,  the  torque  was  turned  off  and  the 
system  allowed  to  drift  for  a  remaining  three  minutes.  This  test  not  only  validated  a 
change  in  torque,  but  also  provided  the  maximum  body  rates  required  for  optimal  control 
analysis.  Results  can  be  seen  in  Figures  48  through  50. 
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Figure  48.  TDRS  Newton-Euler  Spacecraft  Body  Rate  for  V&V  Test  3 
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Figure  50.  TDRS  Newton-Euler  Gimbal  Response  for  V&V  Test  3 
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Figure  5 1 .  TDRS  SimMeehanies  Spaeeeraft  Body  Rate  for  V&V  Test  3 
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Figure  52.  TDRS  SimMechanics  Spacecraft  Body  Angle  for  V&V  Test  3 
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Figure  53.  TDRS  SimMechanics  Gimbal  Response  for  V&V  Test  3 
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The  maximum  spacecraft  body  rotational  rates  were  measured  and  recorded  for 
use  as  optimization  bounds; 

<»i~  =0-0022^, 

®,,_=0.0250^j 

=0-0126^2 

The  final  validation  and  verification  test  was  to  ensure  the  geostationary  rate  of 
the  orbital  frame  had  negligible  impact  on  the  system  and  the  orbital  frame  could  be 
considered  inertial  for  the  limited  timeframe  of  the  antenna  slew.  The  same  five  second 
acceleration  maneuver  above  was  performed,  except  the  spacecraft  was  initialized  at 
0.0042°/s  about  the  y-axis.  The  system  was  then  propagated  for  180  seconds.  The  results 
can  be  seen  in  Figure  54  through  Figure  56,  where  the  plots  depict  motion  relative  to  the 
orbital  frame  (i.e.,  0.0042%  was  subtracted  from  the  spacecraft  body  rate  about  the  y- 
axis). 
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Figure  54.  TDRS  SimMechanics  Spacecraft  Body  Rate  for  V&V  Test  4 
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Figure  55.  TDRS  SimMechanics  Spacecraft  Body  Angle  for  V&V  Test  4 
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Figure  56.  TDRS  SimMechanics  Gimbal  Response  for  V&V  Test  4 
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It  can  be  easily  seen  that  the  differences  between  the  tests  with  and  without  the 
orbital  frame  motion  are  negligible.  The  rate  responses  are  similar  enough  to  diseount  the 
impaet  of  non-inertial  dynamies  and  assume  the  orbital  frame  to  be  inertial.  It  should  be 
noted  that  the  position  plot  response  for  the  y-axis  does  not  have  the  additional  rotation 
rate  removed.  Therefore,  it  is  0.756°  off,  whieh  is  the  expeeted  amount  following  180 
seeonds  of  an  additional  0.0042%. 

With  the  model  validated,  and  useful  bounds  for  body  rate  and  torque  gleaned 
from  the  simulations,  the  model  is  implemented  next  in  DIDO  so  that  the  slew  maneuver 
ean  be  optimized. 

D,  TDRS  OPTIMIZATION  IN  DIDO 

Before  DIDO  eould  be  used  to  determine  optimal  slew  paths,  the  initial  and  final 
conditions  for  the  maneuvers  had  to  be  established.  The  azimuth,  elevation,  azimuth  rate, 
and  elevation  rate  of  a  departing  spaeecraft  were  used  as  the  initial  eonditions  for  the 
problem.  The  azimuth,  elevation,  azimuth  rate,  and  elevation  rate  of  the  future  customer 
were  set  as  the  target  end  eonditions.  These  data  points,  however,  were  not  available  from 
the  literature.  In  order  to  generate  them,  the  Systems  Tool  Kit  (STK)  software  was  used. 
STK  is  a  powerful  orbit  propagator  and  could  easily  provide  azimuth  and  elevation  data 
required  for  this  analysis. 

First,  five  spaeeeraft  were  plaeed  into  an  orbital  model  in  STK;  TDRS,  the 
International  Spaee  Station,  National  Oeeanie  and  Atmospherie  Administration  (NOAA) 
15,  Worldview  2,  and  a  fietional  MEO  spaeeeraft.  Orbital  data  for  these  spaeeeraft  ean  be 
found  in  Table  3.  A  sixth  spaeeeraft  was  added,  a  geostationary  spaeeeraft  at  72° 
outboard  in  azimuth,  was  added  to  test  the  EFOV,  but  did  not  require  STK  simulation  due 
to  its  known  position  and  rate  relative  to  TDRS.  The  assumption  was  also  made  that  both 
spaeeeraft  were  in  the  same  inelination  orbit  and  at  the  aseending  node  during  the 
maneuver  (required  to  nullify  azimuth  rate  sinee  TDRS  is  geosynchronous,  not 
geostationary). 
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TDRS 

ISS 

NOAA-15 

Worldview-2 

MEO 

Semi -Major  Axis  (km) 

42166.41 

6787.36 

7173.81 

7150.06 

16678.14 

Eccentricity 

0.001014 

0.001563 

0.002720 

0.001403 

0.500000 

Inclination  (deg) 

2.730 

51.595 

98.708 

98.504 

20.000 

^RAAN  (deg) 

64.113 

330.089 

330.583 

58.213 

0 

2 

Arg  of  Perigee  (deg) 

187.147 

98.819 

65.831 

74.292 

360 

^Right  Ascension  of  Ascending  Node 

^Argument  of  Perigee 

Table  3.  Target  Satellite  Orbital  Elements 


Six  scenarios  were  identified  over  the  course  of  24  hours  wherein  one  spacecraft 
was  losing  view  of  TDRS,  while  another  was  coming  into  view.  These  included  points 
where  the  MEO  spacecraft  was  at  apogee  in  order  to  simulate  larger  slews.  Azimuth  and 
elevation  data  were  calculated  for  each  of  these  times  and  azimuth  and  elevation  rates 
determined  by  simple  average  of  last  known  angles  over  time.  This  resulted  in  the 
scenarios  listed  in  Table  5. 


Scenario  1 

Scenario  2 

Scenario  3 

Scenario  4 

Scenario  5 

Scenario  6 

Starting  Azimuth  (deg) 

-5.67 

-9.07 

7.52 

-29.42 

-8.84 

-72.00 

Starting  Azimuth  rate  (deg/s) 

-0.0006 

0.0065 

-0.0070 

0.0002 

0.0017 

0.0000 

Ending  Azimuth  (deg) 

-8.27 

-3.83 

8.24 

5.19 

-24.19 

8.24 

Ending  Azimuth  rate  (deg/s) 

0.0054 

-0.0063 

-0.0053 

0.0035 

-0.0007 

-0.0053 

Starting  Elevation  (deg) 

-7.35 

-2.05 

6.12 

-7.21 

-0.86 

0.00 

Starting  Elevation  rate  (deg/s) 

0.0005 

0.0030 

0.0071 

-0.0010 

-0.0077 

0.0000 

Ending  Elevation  (deg) 

-5.16 

8.92 

5.16 

6.97 

1.42 

5.16 

Ending  Elevation  rate  (deg/s) 

-0.0086 

-0.0038 

0.0085 

0.0011 

-0.0012 

0.0085 

Scenario  1:  ISStoNOAA-15 
Scenario  2:  MEO  to  Worlclview-2 
Scenario  3:  NOAA-15  to  Worlclview-2 
Scenario  4:  MEO  to  ISS 
Scenario  5:  ISS  to  MEO 

Scenario  6:  GEO  to  Worlclview-2  (Same  final  conditions  as  Scenario  3) 

Table  4.  Boundary  Conditions  for  Slew  Optimization 


The  data  points  given  in  Table  4  were  then  utilized  in  the  following  problem 
definition; 
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x  =  [o\  4  4  0,  6^]  m={4 

Minimize  J  =  (T^j^ + Tq2  )dt 

^0 

Subject  to: 

ffij  =  [(A -RU-'S)-\r  -RU-^Fj]^ 
e,  =  \{A -Ru-'sy\r  -Ru-'Fj\ 

02  =[(A-RU-'S)-\r -RU-^F'j]^ 

{^o\{tj),0-^ {tj), 62 (^0)5 0\ {io)i 02 (^0))  ~ (O’ ^qiEIq , AzQ,Elj) 
{0jtf),  02{tf),  0jtf),  02{tj^))  =  {AZf,Elf,Az^,El^) 

-\<0jf)<\  deg/5 
-1  <  4(0  ^  1  deg/  s 
-72  <65(0  ^24  deg 
-32  <^2  (0^32  deg 
-0.0022  <ft;i^(0<  0.0022  deg/ 5 
-0.025  <  ct\^{t)  <  0.025  deg/  s 
-0.0126  <ft;sXO^  0.0126  deg/ 5 
0.2<7;,i00^0.2  Nm 
02<TQ2c(f)<^2  Nm 

Figure  57.  TDRS  Optimal  Control  Problem  Definition 


The  bounds  for  the  gimbal  angles  were  taken  directly  from  the  TDRS  FOV  limits. 
Spacecraft  body  rate  bounds  and  torque  limits  were  gleaned  from  simulation  data  as 
explained  in  Section  C.  However,  the  gimbal  rate  limits  were  increased  beyond  0.225°/s. 
If  the  TDRS  gimbal  rate  limits  were  maintained,  there  would  be  almost  no  room  for  slew 
time  improvement.  The  only  possible  decrease  in  slew  time  would  come  from  the  ability 
to  slew  the  antenna  in  such  a  way  as  to  assist  acceleration  and  deceleration.  However,  this 
only  leaves  approximately  10  seconds  of  a  maneuver  for  optimization  and  it  proves  very 
difficult  for  the  system  to  optimize  during  these  accelerations.  Simulations  were 
conducted  to  try  to  optimize  the  slew  while  maintaining  gimbal  rate  limits  and  the 
improvements  were  negligible. 
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However,  if  it  is  assumed  that  the  0.225°/s  rate  limit  is  not  a  hard  limit  (i.e., 
driven  by  the  physical  maximum  rate  of  the  gimbal  hardware),  but  is  instead  a  soft  limit 
(i.e.,  put  in  place  to  minimize  rotational  impact  on  the  spacecraft),  then  this  limit  can  be 
increased  as  long  as  the  condition  it  is  meant  to  alleviate  is  not  violated.  In  other  words, 
the  system  has  a  requirement  to  minimize  rotation  rate  of  the  spacecraft  to  some  degree. 
It  is  assumed  that  the  gimbal  limits  were  established  to  meet  this  requirement.  If  the 
requirement  can  be  met  in  some  other  manner,  slewing  along  an  optimal  path  for 
example,  then  the  rate  and  torque  limits  can  be  increased  and  the  original  requirement  to 
minimize  spacecraft  rotation  maintained.  This  expands  the  solution  space  of  the  system 
and  provides  the  problem  with  ample  resources  for  optimization.  Max  gimbal  rates  were 
set  to  l°/s  in  order  to  provide  a  wide  solution  space.  For  almost  every  maneuver,  this 
boundary  was  sufficient  to  allow  the  antenna  to  maneuver  optimally.  However,  for  larger 
slews,  this  boundary  was  increased  to  2°/s  in  order  to  allow  room  to  smooth  out  the 
trajectory  profile,  as  explained  in  the  following  paragraph. 

Furthermore,  this  problem  formulation  is  similar  to  previous  definitions,  with  one 
exception.  The  addition  of  weighting  factors,  a  and  P,  allowed  for  tailoring  between 
simulations.  The  weighting  factors  allow  for  different  emphasis  to  be  placed  on  the 
endpoint  cost  or  the  mnning  cost.  Each  simulation  was  performed  twice  at  two  different 
weighting  factors.  First,  a  was  set  to  1  and  P  set  to  0.  This  returned  the  time  optimal  slew. 
However,  the  control  sometimes  had  large  impulses  which  may  cause  unacceptable 
vibrations  to  the  spacecraft.  In  order  to  smooth  out  the  control  profile,  a  second 
optimization  was  performed.  For  this  second  optimization,  the  minimum  slew  time  from 
the  first  simulation  was  set  as  a  final  time  for  a  boundary  condition.  Then  a  was  set  to 
zero  to  eliminate  the  endpoint  cost,  and  P  was  set  to  one.  This  forced  DIDO  to  minimize 
the  amount  of  torque  applied  throughout  the  minimum-time  maneuver.  This  resulted  in  a 
much  smoother  torque  trajectory.  However,  less  torque  resulted  in  larger  slew  rates. 
While  these  larger  slew  rates  did  not  adversely  affect  the  output  (i.e.,  the  spacecraft  body 
rotation  limits  were  still  met),  it  was  still  desired  not  to  increase  them  unnecessarily.  For 
one  scenario,  however,  the  gimbal  rate  did  increase  beyond  the  default  l°/s,  so  the  rate 


116 


limit  was  set  to  2°/s  in  order  to  allow  sufficient  solution  space  while  maintaining  a 
smooth  torque  trajectory. 

Scenario  6  was  also  utilized  to  examine  the  ability  to  minimize  the  impact  the 
antenna  slew  has  on  spacecraft  pointing.  An  estimate  for  the  time  of  the  conventional 
maneuver  was  used  as  a  fixed  final  time  and  the  cost  function  adjusted  to  minimize 
spacecraft  spin  rates.  This  scenario  is  indicative  of  a  case  where  timeliness  is  not  a 
driving  concern.  Minimizing  the  spacecraft  spin  rates  would  help  increase  the  accuracy  of 
the  other  payloads  as  well  as  minimizing  fuel  consumption.  If  designed  around  an 
optimal  maneuver,  it  is  possible  that  the  required  momentum  space  of  a  spacecraft  could 
be  reduced,  decreasing  fuel  and  mass  requirements. 

E.  RESULTS 

An  optimal  maneuver  was  solved  for  each  scenario  using  DIDO.  Estimates  were 
made  for  timeliness  of  conventional  slews  based  on  a  maximum  acceleration  for  five 
seconds  to  maximum  rate,  then  maximum  deceleration  over  five  seconds  to  the  final 
desired  rate.  The  longer  axis,  azimuth  or  elevation,  drove  an  estimated  conventional 
maneuver  time.  It  was  assumed  that  the  shorter  axis  would  adjust  the  peak  gimbal  rate  so 
that  both  axes  would  conclude  the  maneuver  concurrently.  These  values  were  then 
compared  to  the  minimal  time  returned  by  DIDO  and  slew  time  improvements  were 
calculated.  The  maximum  azimuth  and  elevation  rates  were  also  noted.  The  data  are 
summarized  in  Table  6. 


Scenario  1 

Scenario  2 

Scenario  3 

Scenario  4 

Scenario  5 

Scenario  6 

Estimated  Conventoinal  Time  (s) 

17.13 

54.32 

9.80 

159.40 

73.78 

362.17 

Optimal  Slew  time  (s) 

16.44 

40.12 

8.66 

87.56 

56.03 

122.60 

Maximum  Azimuth  rate  (deg/s) 

0.2347 

0.2351 

0.1576 

0.4688 

0.4053 

1.0214 

Maximum  Elevation  rate  (deg/s) 

0.1806 

0.3294 

0.2051 

0.2786 

0.0617 

0.0776 

%  reduction  in  slew  time 

4.03 

26.14 

11.62 

45.07 

24.05 

66.15 

Table  5.  Optimal  Antenna  Slew  Results 


The  best  reductions  in  time  come  from  scenarios  where  there  is  a  large  difference 
in  scale  between  azimuth  and  elevation,  and  the  larger  the  dominant  slew,  the  better.  For 
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a  conventional  maneuver,  the  gimbal  rates  are  limited  in  order  to  minimize  spacecraft 
disturbance  due  to  the  antenna  motion.  However,  since  one  axis  is  smaller  than  the  other, 
the  smaller  axis  has  available  room  to  slew  in  a  non-traditional  manner.  This  allows  the 
smaller  axis  to  reduce  the  impact  of  the  antenna  slew  on  the  spacecraft,  allowing  the 
donimant  axis  to  increase  gimbal  rate  without  violating  spacecraft  pointing.  With  the 
exception  of  scenario  three,  the  dominant  axis  gimbal  rate  always  exceeded  the 
conventional  limit,  and  in  some  cases,  the  smaller  axis  gimbal  rate  exceeded  it  as  well. 
Scenario  three  is  an  exception,  but  still  follows  the  same  characteristics  as  the  other 
optimal  slews.  In  scenario  three,  the  largest  slew  magnitude  is  very  small,  less  than  one 
degree,  so  the  antenna  doesn’t  have  time  to  accelerate  to  the  maximum  gimbal  rate. 
Instead  the  antenna  accelerates  for  approximately  half  the  maneuver,  then  decelerates  for 
the  other  half.  However,  since  the  smaller  axis  doesn’t  need  as  much  acceleration  as  the 
dominant  axis,  it  still  has  room  to  assist  the  dominant  axis.  For  each  case,  the  torque  for 
the  dominant  axis  acts  for  the  most  part  as  a  bang-bang  maneuver,  providing  maximum 
torque  to  accelerate  then  maximum  torque  to  decelerate,  with  smaller  deviations  during 
the  middle  duration  of  the  slew.  Alternatively,  the  torque  for  the  smaller  axis  is  changing 
constantly  in  order  to  maintain  the  spacecraft  spin  rates  within  requirements. 

The  full  response  of  scenario  six  is  provided  in  Figure  58  through  Figure  61.  The 
characteristics  of  this  response  are  typical  of  each  scenario;  only  the  sizes  of  the 
individual  maneuvers  and  the  details  of  the  torque  profile  vary,  particularly  with  respect 
to  the  smaller  axis.  For  this  simulation,  final  time  was  set  to  122.6,  a  was  set  to  zero,  and 
P  was  set  to  one.  The  particular  note  is  the  plot  of  the  spacecraft  body  rates,  in  Figure  61. 
Notice  that  the  y-axis,  which  is  the  major  axis  for  this  maneuver,  pushes  the  limits  of  its 
boundary  envelope.  This  was  the  case  for  every  maneuver.  The  spacecraft  rate  was 
always  the  limiting  factor  in  the  maneuver.  Furthermore,  in  some  cases  two  axes  reached 
the  rate  limit,  not  just  the  spacecraft  axis  aligned  with  the  dominant  antenna  slew  axis. 
This  would  indicate  that  the  minor  axis  is  performing  spin  minimization  of  the  major  axis 
to  its  best  ability,  but  is  limited  by  its  own  spin  requirement. 
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Figure  58.  Scenario  6  Optimal  Gimbal  Response 
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Figure  59.  Scenario  6  Optimal  Gimbal  Torque 
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Conventional  Slew  vs  Optimal  Slew 


Figure  60.  Combined  Az-El  Trajectory 


Notice  in  Figure  60  how  the  optimal  path  drifts  away  from  the  conventional  path. 
This  illustrates  how  the  smaller  axis  moves  in  order  to  generate  additional  capability 
along  the  dominant  axis. 
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Figure  61.  Scenario  6  Spacecraft  Body  Spin  Rates 


X  10 


In  order  to  test  the  slew’s  ability  to  minimize  the  induced  spacecraft  rate,  the 
problem  definition  was  altered  slightly,  as  seen  in  Figure  62. 
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x  =  {q  e,  4  ^2)  u  =  {T^, 

tf  tf 

Minimize  J  ^  at +  J  feq/  +  g^Q\y  +  g,o\^)dt 

Subject  to: 

4  =\{A-Ru-^sy\r-Ru-'Fj\ 

(4(^^),  4(^/).  ^i(^/X  ^2(^4)  "  (^f,Elf,  Azj.,El^) 
tj-  =  Conventional  Slew  Time 

-i<  4(0^1  deg/5 
-1<  4(0^1  deg/ 5 
-12<6jt)<2A  deg 
-32  <6/2(0  <32  deg 
-0.0022  <q^(0<  0.0022  deg/ 5 
-0.025  <  (i{y{t)  <  0.025  deg/  s 
-0.0126 <ffii,(0< 0.0126  deg/5 
0.2  <4^(0  <0.2  Nm 
0.2  <7^,200  ^0-2  Nm 

Figure  62.  TDRS  Minimum  Disturbanee  Problem  Definition 


Seenario  six  was  used,  and  the  final  time  was  set  to  the  estimated  eonventional 
slew  time.  Although  the  final  time  eould  be  set  anywhere  from  the  eonventional  slew 
time  to  the  optimal  slew  time,  it  was  set  to  the  eonventional  slew  time  in  order  to  provide 
the  problem  with  the  maximum  room  for  optimizatoin.  Also,  an  additional  running  eost 
and  three  assoeiated  weighting  faetors  were  added  to  minimize  spaeeeraft  spin  rates.  For 
this  simulation,  a  was  set  to  zero,  P  was  set  to  0.01  in  order  to  smooth  torque  without 
impaeting  the  minimization  of  disturbanees.  Lastly,  the  disturbanee  weighting  faetor, 
was  set  to  different  values  in  order  to  tune  on  eaeh  axis.  The  weighting  faetor  for  the  x 
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and  z  axes  was  set  to  1  and  the  weighting  faetor  for  the  y  axis  was  set  to  10  in  order  to 
minimize  disturbanees  on  the  axis  impaeted  most  by  the  motion  of  the  antenna.  The 
optimized  response  ean  be  seen  in  Figure  63  through  Figure  65.  The  modulation  of  the 
eontrol  torques,  as  seen  in  Figure  64,  is  neeessary  to  eounteract  the  motion  induced  on  the 
spacecraft  body. 
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Figure  63.  TDRS  Scenario  6  Minimum  Disturbance  Gimbal  Response 
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Azimuth  Torque  (Nm) 
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Figure  64.  TDRS  Scenario  6  Minimum  Disturbance  Torque  Profile 
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Figure  65.  TDRS  Scenario  6  Minimum  Disturbance  Spacecraft  Spin  Rate 

Even  with  optimization,  the  spin  rate  does  reach  rates  that  approach  the  boundary 
rate  (0.0208  °/s).  As  opposed  to  the  minimum  time  maneuvers,  the  system  never  reaches 
the  rate  constraint  and  recovers  to  reduce  the  total  spin  rate  throughout  the  entire 
maneuver.  However,  this  is  performed  at  the  cost  of  additional  spin  rate  in  the  x  and  z 
axes.  Adjustments  to  the  cost  function  and  weighting  factors  could  reduce  this  imbalance 
to  yield  a  desired  momentum  minimization.  This  response  can  be  compared  to  the 
conventional  maneuver,  as  seen  in  Figure  66. 
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Figure  66.  TDRS  Scenario  6  Conventional  Slew 

The  conventional  slew  imparts  much  less  spin  on  the  x  and  z  axes,  but  the  spin 
rate  along  the  y-axis  is  near  or  at  the  limit  for  the  majority  of  the  maneuver.  Figure  67 
and  Figure  68  show  the  slew  magnitudes  of  the  spacecraft  rates.  It  is  evident  that  the 
optimal  maneuver  greatly  reduces  the  disturbance  on  the  spacecraft. 
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Spacecraft  Rate  Magnitude 


Figure  67.  TDRS  Scenario  6  Minimum  Disturbance  Spacecraft  Rate  Magnitude 


Spacecraft  Rotation  Magnitude 


Figure  68.  TDRS  Scenario  6  Conventional  Slew  Spacecraft  Rate  Magnitude 

F.  IMPLEMENTATION  ON  LABORATORY  TESTBED 

In  order  to  test  optimal  control  slews  in  a  controlled  environment,  a  modular 
testbed  was  designed,  built,  and  tested  by  Lt  Greg  Contreras,  USN  [23].  The  testbed 
consists  of  a  lightweight,  transportable  base,  a  support  arm,  small  antenna  dish,  and 
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associated  power  and  control  systems.  The  dish  can  rotate  about  the  elevation  axis,  and 
the  support  arm  rotates  to  provide  azimuth  pointing.  Both  axes  are  driven  by  brushless 
DC  motors  built  by  Maxon.  Azimuth  is  driven  by  a  gear  connected  to  the  bottom  of  the 
support  arm.  Elevation  is  driven  directly  by  a  gimbal  support  piece  rigidly  fixed  to  the 
antenna.  The  motors  controlled  by  Maxon  EPOS2  controllers  and  the  entire  system  can 
be  controlled  in  realtime.  The  testbed  was  designed  to  be  eompletely  modular,  and 
various  end  effectors  could  be  replaced  in  order  to  simulate  a  multitude  of  designs  or 
missions,  including  communications  antennas,  telescopes,  or  weapon  systems.  Lastly,  the 
system  is  lightweight,  portable,  and  can  be  operated  remotely.  Further  details  of  the 
system,  its  design,  and  its  operation  can  be  found  in  [23]. 


Figure  69.  Azimuth-Elevation  Laboratory  Testbed  [23] 


The  optimal  trajectrory  provided  by  DIDO  for  scenario  3  was  implemented  on  the 
laboratory  testbed.  The  scenario  called  for  a  0.7183°  change  in  azimuth  and  0.9548° 
change  in  elevation  and  was  optimized  for  a  slew  time  of  8.66  seconds,  as  depicted  in 
Figure  70. 
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Figure  70.  TDRS  Scenario  3  Optimal  Gimbal  Trajectory 

In  order  to  implement  this  slew  on  TDRS,  the  trajectory  would  have  to  be 
converted  to  a  format  that  the  TDRS  control  system  can  process.  This  involves  fitting  the 
position  data  to  a  third  order  polynomial.  The  position  data  for  azimuth  and  elevation 
were  fit  to  a  polynomial  using  the  MATLAB  function,  polyfit.  This  resulted  in  two 
equations: 

6»i  =-0.0025t'+0.033fi'-0.0182t  +  7.5201(deg)  (7.1) 

6»2  =0.033t'-0.045fiV0.0346t  +  6.1091(deg)  (7.2) 

Comparing  the  optimal  trajectory  with  polynomial  trajectories,  shown  in 
Figure  71,  some  differences  can  be  found.  There  are  losses  in  fidelity  that  occur  when  the 
system  is  fit  using  a  third  order  polynomial.  Further  work  would  need  to  be  done  to 
explore  ways  to  mitigate  this  loss.  This  could  include  adjusting  the  cost  function  to  obtain 
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smoother  functions,  implementing  a  path  file  that  could  drive  a  third  order  polynomial 
response,  or  finding  innovative  ways  to  provide  TDRS  with  the  more  complex  trajectory. 


Azimuth  Angle 


Elevation  Angle 


Figure  71.  TDRS  Scenario  6  Optimal  &  Polynomial  Slew  Comparison 


These  equations  were  then  evaluated  over  the  slew  time  at  discrete  intervals  of 
0.05  seconds.  This  was  the  required  position  data  format  for  the  testbed.  This  data  was 
converted  to  quad  counts  (462848  quad  counts  per  360°)  and  saved  into  a  .csv  (comma 
separated  values)  file,  which  could  be  read  by  the  testbed  controllers. 

The  testbed  was  configured  to  the  position  control  mode  and  provided  the  optimal 
control  path  as  described  in  [23].  This  resulted  in  the  responses  in  Figure  72  and  Figure 
73.  The  red  lines  indicate  the  demanded  position.  The  black  lines,  overlaid  over  the 
demanded  position,  represent  the  actual  position.  Both  demanded  and  actual  position  is 
measured  in  quad  counts  on  the  vertical  axis. 
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■  Position  Demand  Value  ■Velodty  Actual  Value  Averaged  T«nrlTi~i  [ms] 

■  Position  Actual  Value 

Figure  72.  Experimental  Implementation  of  Optimal  Antenna  Slew  (Azimuth) 


Figure  73.  Experimental  Implementation  of  Optimal  Antenna  Slew  (Elevation) 
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The  red  line  in  Figure  79  and  Figure  80  depiets  the  demanded  position  fed  to  the 
testbed.  The  blaek  line  represents  the  aetual  response  generated  by  the  testbed.  As  ean  be 
seen,  the  aetual  response  elosely  matehes  the  desired  trajeetory.  Overall,  this  test 
demonstrated  the  ability  to  generate  an  optimal  slew  maneuver  and  to  steps  required  to 
suoeessfully  implement  this  maneuver  on  a  physieal  system. 
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VIII.  CONCLUSION 


A,  CONCLUSION 

This  thesis  demonstrated  the  improvements  to  spacecraft  antenna  slew  maneuvers 
that  can  be  obtained  by  utilizing  optimal  control  theory.  The  primary  driver  of  these 
improvements  is  a  shift  in  problem  formulation.  Traditionally,  the  antenna  control  system 
is  designed  in  such  a  way  as  to  minimize  its  impact  on  spacecraft  pointing.  In  order  to 
meet  spacecraft  pointing  requirements,  conservatism  is  often  built  into  the  maneuver 
design  in  order  to  limit  the  impact  on  the  spacecraft.  For  TDRS  this  resulted  in  a  gimbal 
rate  limit  that  greatly  reduced  the  system’s  responsiveness.  However,  if  the  problem  is 
attacked  from  a  different  angle,  the  unnecessary  conservative  can  be  removed.  Simply  by 
a  change  in  the  design  point  of  view,  additional  capability  can  be  gleaned  from  the 
system. 

It  was  discovered  that  the  greatest  benefit  arose  for  large  maneuvers  and 
maneuvers  with  a  large  size  difference  between  each  axis.  This  result  particularly 
benefits  maneuvers  in  the  EFOV  where  user  time  is  currently  being  minimized  or  lost. 
Nonetheless,  even  small  maneuvers  can  be  improved.  By  applying  optimal  control,  it  is 
possible  to  reduce  the  worst-case  slew  time,  meaning  that  additional  capacity  can  be 
attained  with  minimal  impact  to  scheduling.  This  could  drastically  reduce  the  inefficiency 
of  slew  times  and  increase  operational  availability  of  TDRS  while  minimizing  costly 
software  and  hardware  updates. 

Lastly,  the  results  show  that  minimizing  momentum  transfer  to  the  spacecraft 
from  the  antenna  can  be  achieved  as  part  of  the  optimal  control  solution.  This  can  have 
impact  on  the  design  of  future  TDRS  bus  systems,  specifically  by  relaxing  the 
requirements  on  the  ADCS. 

These  results  do  not  apply  solely  to  antenna  pointing.  In  effect,  this  theory  can 
apply  to  any  pointing  system;  robotic  manipulators,  optics,  even  weapon  systems. 
Furthermore,  it  can  be  applied  on  systems  other  than  spacecraft. 
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B,  FUTURE  WORK 

The  scope  of  this  thesis  was  to  investigate  the  feasibility  of  optimal  antenna  slews. 
This  limited  the  extent  that  thorough  analysis  could  be  performed,  especially  with  the 
available  spacecraft  data.  Furthermore,  while  the  basic  concepts  and  functionality  were 
thoroughly  vetted,  many  assumptions  were  made  to  minimize  the  complexity  of  the 
model.  In  order  to  implement  this  type  of  maneuver  on  a  spacecraft,  a  fully  complete  and 
high  fidelity  model  would  need  to  be  utilized.  This  would  include  flexibility,  sloshing, 
vibration,  static  and  dynamic  friction,  non-inertial  affects,  external  disturbances,  etc. 

Furthermore,  more  detailed  knowledge  of  the  conventional  slew  maneuver  profile 
could  lead  to  further  benefits.  The  slew  method  indicated  in  literature  does  not  account 
for  the  nominal  three  minute  slew  window  that  TDRS  utilizes.  For  example,  the  three 
minute  TDRS  slew  window  is  based  on  the  maximum  maneuver  size  within  the  LEO 
FOV,  28°.  However,  for  maximum  acceleration  to  maximum  gimbal  rate,  this  would  only 
account  for  a  130  second  slew.  This  leaves  50  seconds  of  slew  time  that  is  being  utilized 
by  TDRS  in  some  unknown  way.  Because  of  this,  it  is  very  possible  that  even  more 
improvements  could  be  found  within  the  full  slew  maneuver. 

Above  all,  the  spacecraft  ADCS  would  need  to  be  accurately  modeled.  TDRS 
ADCS  was  not  modeled  in  this  thesis  due  to  unknowns  in  its  sizing  and  control 
algorithms.  However,  if  these  details  were  known  and  modeled,  then  assumptions  could 
be  minimized  and  more  accurate  ADCS  requirements  incorporated  into  the  problem 
definition  to  yield  a  more  applicable  result.  Furthermore,  the  minimum  time  and 
minimum  momentum  maneuvers  could  be  combined  to  yield  a  single  problem  definition 
that  could  provide  more  system  availability,  while  maintaining  or  exceeding  spacecraft 
pointing  requirements. 

Once  thoroughly  vetted  on  a  high  fidelity  spacecraft  model,  operational  analysis 
would  need  to  be  conducted.  Currently,  TDRS  is  meeting  its  defined  mission 
requirements.  Additionally,  even  though  TDRS  performs  upwards  of  50  slews  a  day, 
there  are  certain  periods  where  the  system  is  not  being  utilized  due  to  lack  of  user 
requests.  Therefore,  if  the  operational  analysis  were  performed  over  an  entire  day  or  a 
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long  period  of  time,  the  benefits  would  be  lost  if  averaged  out.  It  is  imperative  that 
operational  analysis  foeus  on  missions  that  are  not  being  met  or  operations  impaeted  by 
extended  slew  times  that  eould  be  solved  with  the  applieation  of  optimal  eontrol.  Further 
foeus  should  be  paid  to  times  of  high  eongestion  where  the  need  for  timeliness  is  highest 
and  the  mission  impaet  of  delayed  aequisition  the  most  oritieal. 

Lastly,  the  ability  of  the  desired  system  to  perform  these  types  of  maneuvers  must 
be  evaluated.  This  will  be  a  system  speeifie.  Optimal  maneuvers  may  be  easier  to 
implement  on  higher  order  polynomials  or  different  eontrol  sehemes  altogether.  TDRS 
utilizes  a  third  order  polynomial,  and  this  led  to  a  loss  of  some  preeision  regarding  the 
response.  While  it  is  possible  the  system  could  still  perform  the  required  optimal 
maneuver  under  these  constraints,  it  cannot  be  discounted  that  the  very  characteristics 
that  assist  the  maneuver  could  be  lost.  There  are  ways  around  this  problem,  such  as 
incorporating  the  control  scheme  in  problem  formulation,  or  innovate  ways  to  trick  the 
system  into  accepting  a  more  complex  command,  as  demonstrated  in  the  TRACE  flight 
experiment  [25].  For  systems  in  development,  control  schemes  necessary  for  optimal 
maneuvers  should  be  implemented  early  in  design.  This  would  allow  for  the  best  use  of 
the  hardware  and  for  the  best  vehicle  performance. 
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APPENDIX  A.  GENERAL  MATLAB  CODE  FOR  THREE  LINK 

SYSTEM 


The  following  is  an  example  MATLAB  seript  that  implements  the  Netwon-Euler 
proeess  deseribed  in  this  thesis.  The  eode  illustrates  how  to  implement  a  double¬ 
pendulum  model.  As  was  mentioned  in  the  text,  it  is  straightforward  to  modify  the  eode 
for  different  eonfigurations.  Plaeeholder  values  have  been  inserted  for  neeessary 
parameters  such  as  mass  and  inertia.  This  script  can  be  used  both  to  simulate  the  system 
and  as  part  of  an  optimization  routine. 

%  Simple  three  body  system 

function  Three -Body-Newt on-Euler-Code 

g,  q, 
o  o 

clear  all;  clc;  %close  all; 

tic;  %start  program  timer 

%%Establish  main  variables 
g  =  9.81;  %gravity  m/s''2 

%ACRONYMS 

%TROC  =  TIME  RATE  OF  CHANGE 
%DCM  =  Direction  Cosine  Matrix 


9-  9- 
o  o 


%T0  TAILOR  SYSTEM,  COMMENT  OUT  NON-CONSTRAINED  ANGLES  AND  ANGLE  RATES 
%CONSTRAINED  ANGLE  RATES  SHOULD  BE  SET  TO  ZERO 

%A11  gimbal  variables  defined  in  outer  link  and  assumed  (+)  in  outer 


link 

%comment  out  unconstrained  axes 
syms  tlx  tly  tlz; 
syms  ulx  uly  ulz; 
gimbal  1  (rad/s) 
tlx  =  0; 
tly  =  0; 

%tlz  =  0; 


%orientation  angles  of  gimbal  1  (rad) 
%rate  of  change  of  orientation  angle  of 


%angle  of  gimbal  1 
%angle  of  gimbal  1 
%angle  of  gimbal  1 


in  X  direction 
in  y  direction 
in  z  direction 


ulx  =  0; 
uly  =  0; 
%ulz  =  0; 


%TROC  of  angle  of  gimbal  1  in  x  direction 
%TROC  of  angle  of  gimbal  1  in  Y  direction 
%TROC  of  angle  of  gimbal  1  in  Z  direction 


%Kinematic  differential  equation  for  link  1  (Kane,  Likins,  Levinson  pg 
427 

Gammal  =  [cos (tly) *cos (tlz) ,  sin(tlz),  0; 

-cos (tly) *sin (tlz)  ,  cos(tlz),  0; 
sin (tly)  ,  0,1]; 
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%Tiine  rate  of  change  of  kinematic  differential  equation  for  link  1 
Gammaldot  =  [ (-sin (tly) *cos (tlz) *uly)  -  (cos (tly) *sin (tlz) *ulz)  , 
cos(tlz)*ulz,  0; 

(sin (tly) *sin (tlz) *uly)  -  (cos (tly) *cos (tlz) *ulz)  ,  - 

sin (tlz) *ulz,  0; 

cos (tly) *uly  , 

0,  0]; 

ul  =  [ulx;  uly;  ulz] ;  %TROC  of  angle  of  gimble  1  in  vector  form 


%TO  TAILOR  SYSTEM,  COMMENT  OUT  NON-CONSTRAINED  ANGLES  AND  ANGLE  RATES 
%CONSTRAINED  ANGLE  RATES  SHOULD  BE  SET  TO  ZERO 

%A11  gimbal  variables  defined  in  outer  link  and  assumed  (+)  in  outer 


link 

%comment  out  unconstrained  axes 
syms  t2x  t2y  t2z; 
syms  u2x  u2y  u2z; 
gimbal  2  (rad/s) 
t2x  =  0; 
t2y  =  0; 

%t2z  =  0; 


%orientation  angles  of  gimbal  2  (rad) 
%rate  of  change  of  orientation  angle  of 

%angle  of  gimbal  2  in  x  direction 
%angle  of  gimbal  2  in  y  direction 
%angle  of  gimbal  2  in  z  direction 


u2x  =  0; 
u2  y  =  0 ; 
%u2  z  =  0 ; 


%TROC  of  angle  of  gimbal  2  in  x  direction 
%TROC  of  angle  of  gimbal  2  in  Y  direction 
%TROC  of  angle  of  gimbal  2  in  Z  direction 


%Kinematic  differential  equation  for  link  2  (Kane,  Likins,  Levinson  pg 
427 

Gamma2  =  [cos (t2y) *cos (t2z) ,  sin(t2z),  0; 

-cos (t2y) *sin (t2z) ,  cos(t2z),  0; 
sin (t2y)  ,  0,1]; 


%Time  rate  of  change  of  kinematic  differential  equation  for  link  2 
Gamma2dot  =  [ (-sin (t2y) *cos (t2z) *u2y)  -  (cos (t2y) *sin (t2z) *u2z) , 
cos  (t2z) *u2z,  0; 

(sin (t2y) *sin (t2z) *u2y)  -  (cos (t2y) *cos (t2z) *u2z)  ,  - 

sin (t2z) *u2z,  0; 

cos (t2y) *u2y  , 

0,  0]; 


u2  =  [u2x;  u2y;  u2z];  %TROC  of  angle  of  gimble  1  in  vector  form 

%%Direction  Cosine  matrices 

%ONLY  APPIICABLE  FOR  THIS  PROBLEM,  NOT  GENERALIZED 
%DCMS  12  and  23  ARE  FROM  KANE, LIKINS, LEVINSON  PG  423 


%Body-three:  1-2-3 

%Transf ormation  matrix  from  1  to  2 

DCM_21  =  [cos (tly) *cos (tlz)  ,  sin (tlx) *sin (tly) *cos (tlz)  + 

cos (tlx) *sin (tlz)  ,  -cos (tlx) *sin (tly) *cos (tlz)  +  sin  (tlx) *sin (tlz) ; 
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-cos (tly) *sin(tlz) 
cos (tlz) *cos (tlx) ,  cos (tlx) 
sin (tly) 
sin (tlx) *cos (tly) , 


-sin (tlx) *sin (tly) *sin (tlz) 
sin (tly) *sin (tlz)  +  sin(tlx) 

cos (tlx) 


+ 

*cos  (tlz) ; 
*cos (tly) ] ; 


%Transf ormation  matrix  from  from  2  to  3 

DCM_32  =  [cos (t2y) *cos (t2z)  ,  sin (t2x) *sin (t2y) *cos (t2z) 

cos (t2x) *sin (t2z) ,  -cos (t2x) *sin (t2y) *cos (t2z)  +  sin(t2x) 

-cos (t2y) *sin(t2z) ,  -sin(t2x) *sin(t2y) *sin(t2z) 
cos (t2z) *cos (t2x)  ,  cos (t2x) *sin (t2y) *sin (t2z)  +  sin(t2x) 

sin(t2y)  , 

sin (t2x) *cos (t2y)  ,  cos (t2x) 


+ 

*sin (t2z) ; 

+ 

*cos  (t2z) ; 
*cos (t2y) ] ; 


syms  ql  q2  q3  q4 
ql  =  0; 
q2  =  0; 
q3  =  0; 
q4  =  1; 

q  =  [ql;q2;q3;q4] ; 


%Quaternion  1 ; 
%Quaternion  2; 
%Quaternion  3; 
%Quaternion  4 ; 


comment  if  non-inertial  body  1 
comment  if  non-inertial  body  1 
comment  if  non-inertial  body  1 
comment  if  non-inertial  body  1 


syms 

qdotl 

qdotl 
=  0; 

qdot2  qdot3  qdot3 
%TROC 

of 

ql;  comment 

if 

non-inertial 

body 

1 

qdot2 

=  0; 

%TROC 

of 

q2 ;  comment 

if 

non-inertial 

body 

1 

qdot3 

=  0; 

%TROC 

of 

q3;  comment 

if 

non-inertial 

body 

1 

qdot4 

=  0; 

%TROC 

of 

q4;  comment 

if 

non-inertial 

body 

1 

qdot 

=  [qdotl ; qdot2 ; qdot3 ; qdot3 ]  ; 

%Transf ormatrion  matrix  from  N  to  1  (BONG  WIE  pg  335) 

DCM  IN  =  [1-2* (q2^2  +  q3^2)  ,  2 * ( ql *q2  +  q3 *q4 )  ,  2* (ql*q3  +  q2*q4 )  ; 

2*  (q2*ql  +  q3*q4)  ,  1-2  *  ( ql  ^2  +  q3 '^2  )  ,  2*(q2*q3  +  ql*q4); 
2* (q3*ql  +  q2*q4)  ,  2* (q3*q2  +  ql*q4 )  ,  1-2  * ( ql ^2  +  q2 ^2 )  ] ; 


%Additional  DCMs  based  on  primary  DCMs 


DCM 

12 

=  DCM 

21.'  ; 

DCM 

"2  3 

=  DCM 

32  .  '  ; 

DCM 

"ni 

=  DCM 

'in.  '  ; 

DCM 

2N 

=  DCM 

21*DCM 

IN; 

DCM 

'n2 

=  DCM 

’2N.  '  ; 

DCM 

3N 

=  DCM 

32*DCM 

2N; 

DCM 

’n3 

=  DCM 

’3N .  '  ; 

DCM 

31 

=  DCM 

32*DCM 

_21; 

DCM 

"13 

=  DCM 

'31.'  ; 

%%Rotational  Velocities 

%Angular  rate  of  body  1  relative  to  inertial  in  frame 

( rad/ s )?????????  FRAME 
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%?????????????????????????????????????????????????????????????????????? 
??????????????????????????????????????????????????????????????????????? 
???????????????????????????????????????? 
syms  wlx  wly  wlz 

wl  =  [wlx;  wly;  wlz];  %angular  rate  of  body  1  in  vector  form 

%cross  product  matrix  for  (wl  X  _) 
wlc  =  [0  ,  -wlz,  wly; 

wlz,  0,  -wlx; 

-wly,  wlx,  0 ] ; 

%double  cross  product  matrix  for  (wl  X  (wl  X  _) ) 
wlcc  =  [- (wly^2) - (wlz^2) ,  wlx*wly, 

wly*wlx,  - (wlz^2) - (wlx^2) , 
wlz*wlx,  wlz*wly,  -(wlx^2) 


wlx*wl z ; 
wly*wl z ; 

- (wly^2) ] ; 


%Angular  rate  of  body  2  relative  to  body  1  in  body  2  frame  (rad/s) 
w2  =  DCM  21*wl  +  Gammal*ul; 
w2x  =  w2 ( 1 ) ;  w2y  =  w2(2);  w2z  =  w2(3); 

%cross  product  matrix  for  (w2  X  _) 
w2c  =  [0  ,  -w2z,  w2y; 

w2z,  0,  -w2x; 

-w2y,  w2x,  0 ] ; 

%double  cross  product  matrix  for  (w2  X  (w2  X  _) ) 
w2cc  =  [- (w2y^2) - (w2z^2) ,  w2x*w2y, 

w2y*w2x,  -  (w2z"'2)  -  (w2x^2)  , 
w2z*w2x,  w2z*w2y,  -(w2x^2) 


w2x*w2  z ; 
w2y*w2  z ; 

- (w2y^2) ] ; 


%Angular  rate  of  body  2  relative  to  body  1  in  body  2  frame  (rad/s) 
w3  =  DCM  32*w2  +  Gamma2*u2; 
w3x  =  w3(l);  w3y  =  w3(2);  w3z  =  w3(3); 

%cross  product  matrix  for  (w3  X  _) 
w3c  =  [0  ,  -w3z,  w3y; 

w3z,  0,  -w3x; 

-w3y,  w3x,  0] ; 

%double  cross  product  matrix  for  (w3  X  (w3  X  _) ) 
w3cc  =  [- (w3y^2) - (w3z^2) ,  w3x*w3y, 

w3y*w3x,  - (w3z^2 ) - (w3x^2 ) , 
w3z*w3x,  w3z*w3y,  -(w3x^2) 


w3x*w3z; 

w3y*w3z; 

- (w3y^2) ]  ; 


%%Inertia  Properties 
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ml  = 

10; 

%mass  of 

body 

1 

(kg) 

m2  = 

10; 

%mass  of 

body 

2 

(kg) 

m3  = 

10; 

%mass  of 

body 

3 

(kg) 

Ml  = 

ml*eye (3) ; 

%mass 

matrix 

for 

body 

M2  = 

m2  * eye ( 3 ) ; 

%mass 

matrix 

for 

body 

M3  = 

m3*eye  ( 3 ) ; 

%mass 

matrix 

for 

body 

zero 

=  0*eye  ( 3 ) ; 

%3x3 

zero  placeholder 

1 

2 

3 

for  matrices 


II  =  eye (3);  %replace  with  inertia  matrix 
%moments  of  inertia  of  body  1  in  frame  1  (kg  m'^2) 


%moments  of  inertia  of  2nd  body  in  frame  2 
12  =  eye (3);  %replace  with  inertia  matrix 
%moments  of  inertia  of  body  2  in  frame  2  (kg  m^2) 


%moments  of  inertia  of  3rd  body  in  fame  3 
13  =  eye (3);  %replace  with  inertia  matrix 
%moments  of  inertia  of  body  3  in  frame  3  (kg  m^2) 


HI  =  Il*wl;  %Angular  momentum  of  body  1  in  frame  1 

H2  =  I2*w2;  %Angular  momentum  of  body  2  in  frame  2 

H3  =  I3*w3;  %Angular  momentum  of  body  3  in  frame  3 

%syms  11  12 
%%Radii 

%radius  of  CG  of  body  1  to  gimbal  1  in  frame  1 
rllx  =  0;  rlly  =  0;  rllz  =  0; 

rll  =  [rllx;  rlly;  rllz];  %radius  of  CG  of  body  1  to  gimbal  1 

rile  =  [0  ,  -rllz,  rlly; 

rllz,  0,  -rllx; 

-rlly,  rllx,  0]; 

%radius  from  cml  to  gimbal  1  cross  product 


%radius  of  CG  of  body  2  to  gimbal  1 
syms  r21x  r21y  r21z 

r21x=  R21(l);  r21y  =  R21(2);  r21z  = 
r21  =  [r21x;  r21y;  r21z];  %radius 


in  frame  2 
R2 1 ( 3 )  ; 

of  CG  of  body  2  to  gimbal 


1 


r21c  =  [0  ,  -r21z,  r21y; 

r21z,  0,  -r21x; 
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-r2 ly,  r2 lx,  0 ] ; 

%radius  from  cm2  to  gimbal  1  cross  product 


%radius  of  CG  of  body  2  to  gimbal  2  in  frame  2 
syms  r22x  r22y  r22z 

r22x  =  R22(l);  r22y  =  R22(2);  r22z  =  R22(3); 

r22  =  [r22x;  r22y;  r22z];  %radius  of  CG  of  body  2  to  gimbal  2 

r22c  =  [0  ,  -r22z,  r22y; 

r22z,  0,  -r22x; 

-r22y,  r22x,  0 ]  ; 

%radius  from  cm2  to  gimbal  2  cross  product 


%radius  of  CG  of  body  3  to  gimbal  2  in  frame  3 
syms  r32x  r32y  r32z 

r32x  =  R32(l);  r32y  =  R32(2);  r32z  =  R32(3); 

r32  =  [r32x;  r32y;  r32z];  %radius  of  CG  of  body  3  to  gimbal  2 

r32c  =  [0  ,  -r32z,  r32y; 

r32z,  0,  -r32x; 

-r32y,  r32x,  0] ; 

%radius  from  cm3  to  gimbal  2  cross  product 


%External  forces  on  body  1 

syms  Fix  Fly  Flz 

Fix  =  0;  Fly  =  0;  Flz  =  0; 

FI  =  [Fix;  Fly;  Flz]; 

%T0  SOLVE  FOR  BASE  FORCE,  USE  SYMS  FIX  AND  FlY,  THEN  SOLVE  FOR  XDOT 
BELOW 

%AND  PULL  OUT  VX  AND  VY . 

%THEN  USE  THE  FOLLOWING  LINE: 

%solutions  =  solve (Vx  ==  0,  Vy  ==  0,  Fix,  Fly) 

%TO  SOLVE  FOR  FIX  AND  FlY,  THEN  COPY  PASTE 


%External  forces  on  body  2 
F2x  =  0;  F2y  =  -g*m2;  F2z  =  0; 
F2  =  [F2x;  F2y;  F2z]; 

%External  forces  on  body  3 
F3x  =  0;  F3y  =  -g*m3;  F3z  =  0; 
F3  =  [F3x;  F3y;  F3z]; 

%External  torques  on  body  1 
Tlx  =  0;  Tly  =  0;  Tlz  =  0; 

T1  =  [Tlx;  Tly;  Tlz]; 

%External  torques  on  body  2 
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T2x  =  0;  T2y  =  0;  T2z  =  0; 

T2  =  [T2x;  T2y;  T2z]; 

%External  torques  on  body  3 
T3x  =  0;  T3y  =  0;  T3z  =  0; 

T3  =  [T3x;  T3y;  T3z]; 

syms  Fglx  Fgly  Fglz;  %Forces  at  gimbal  1 

Fgl  =  [Fglx;  Fgly;  Fglz]; 

syms  Fg2x  Fg2y  Fg2z;  %Forces  at  gimbal  2 

Fg2  =  [Fg2x;  Fg2y;  Fg2z]; 

%Torques  at  gimbal  1 
Tglx  =  0; 

Tgly  =  0; 

Tglz  =  0; 

Tgl  =  [Tglx;  Tgly;  Tglz] ; 

%Torques  at  gimbal  2 
Tg2x  =  0; 

Tg2y  =  0; 

Tg2  z  =  0 ; 

Tg2  =  [Tg2x;  Tg2y;  Tg2z]; 


%%Fstablish  main  matrices 

%Each  row  of  I  uses  the  frame  corresponding  to  the  leading  column: 
%i.e.,  row  1  is  frame  1,  row  2  is  frame  2,  row  3  is  frame  3,  etc... 
I  =  [II,  zero,  zero; 

I2*DCM  21,  I2*Gammal,  zero; 

I3*DCM~31,  I3*DCM_32*Gammal,  I3*Gamma2]; 

%Each  row  of  u  uses  the  inertial  reference  frame 
u  =  [M1*DCM  Nl;  M2*DCM  Nl;  M3*DCM  Nl]; 


%Each  row  of  PI  uses  the  inertial  reference  frame 
PI  =  [zero 


zero,  zero; 

m2*DCM_N2*r21c*DCM_21  -  m2 *DCM_N1 *rl Ic 

f 

m2*DCM  N2*r21c*Gammal,  zero; 

m3*DCM_N3*r32c*DCM_Nl  -  m3*DCM_N2*r22c*DCM_21  + 
m3*DCM_N2*r21c*DCM_21  -  m3*DCM_Nl*rllc,  m3*DCM_N3*r32c*DCM_32*Gammal 
m3*DCM  N2 *r22c*Gammal  +  m3*DCM  N2 *r2 lc*Gammal ,  m3*DCM  N3*r32c*Gamma2 ] 

%Each  row  of  T  uses  the  frame  corresponding  to  that  row 
T  =  [T1  -  wlc*Hl  -  DCM_12*Tgl; 

T2  -  w2c*H2  +  Tgl  -  (DCM_23*Tg2)  -  12 *Gammaldot*ul  - 
I2*DCM  21*wlc*DCM  12 *Gammal *ul ; 
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T3  -  w3c*H3  +  Tg2  -  I3*DCM  32 *Gammaldot*ul  -  I3*Gamma2dot*u2  - 
I3*DCM  31*wlc*DCM  13*Gamma2*u2  -  I3*cross (DCM  32 *Gammal *ul , 

Gamma2  *u2 ) ] ; 


%Each  row  of  F  uses  the  inertial  reference  frame 
F  =  [FI; 

F2  -  m2*DCM  N2 *r2 lc*Gammaldot*ul  -  m2*DCM  Nl*wlcc*rll  + 
m2*DCM_N2*w2cc*r21  -  m2*DCM_N2*r21c*DCM_21*wlc^DCM_12*Gammal*ul; 

F3  -  m3*DCM  N2 *r2 lc*Gammaldot*ul  +  m3*DCM  N2 *r22c*Gammaldot*ul  - 
m3*DCM  N3*r32c*DCM  32*Gammaldot*ul  -  m3* (DCM  N3*r32c*Gamma2dot*u2 )  - 
m3* (DCM_Nl*wlcc*rll)  +  m3* (DCM_N2*w2cc*r21)  -  m3* (DCM_N2*w2cc*r22)  + 
m3* (DCM_N3*w3cc*r32)  -  m3* (DCM_N3*r32c*DCM_31*wlc  + 

DCM_N2*r21c*DCM_21*wlc  -  DCM_N2*r22c*DCM_21*wlc) *DCM_12*Gammal*ul  - 
m3*DCM_N3*r32c*DCM_31*wlc*DCM_13*Gamma2*u2  - 
m3*DCM  N3*r32c*cross (DCM  32*Gammal*ul, Gamma2*u2) ] ; 


%Each  row  of  P  uses  the  frame  corresponding  to  that  row 
P  =  [rllc*DCM_lN  ,  zero; 

-r21c*DCM_2N,  r22c*DCM_2N; 
zero  ,  -r32c*DCM  3N] ; 


J 


[eye (3)  , 
-eye (3) , 
zero. 


zero; 
eye (3) ; 
-eye  (3) ] ; 


%Restructured  A  matrix 
A  =  [I,  [ zero; zero; zero] ; 

PI  (1:3,  :) ,  u(l:3,  :) ]  ; 


S  =  [ PI ( 4 : end, : ) ,  u(4:end, :)]; 

R  =  [P; 

J(l:3,  :) ]  ; 

U  =  J ( 4 : end,  : ) ; 

Tprime  =  [T; F ( 1 : 3 ,  :  ) ]  ; 

Fprime  =  F(4:end, :); 

%ELEM  vector  denotes  only  the  unconstrained  variables  -  see  numbers 
below 

%  %  %  wlx  =  1 
%  %  %  wly  =  2 
%  %  %  wlz  =  3 
%  %  %  ulx  =  4 
%  %  %  uly  =  5 
%  %  %  ulz  =  6 
%  %  %  u2x  =  7 
%  %  %  u2y  =  8 
%  %  %  u2z  =  9 
%  %  %  vlx  =  10 
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%  %  %  vly  =  11 
%  %  %  viz  =  12 

%example,  an  inertial  link  1  (wl,vl=0),  and  only  az  (y)  in  gimbal  1  and 

%el  (z)  in  gimbal  two  would  be  ELEM  =  [5,9] 

%example,  inertial  link  1  with  double  pendulum  (both  gimbals  in  z) 

%  ELEM  =  [6,9] 

ELEM  =  [6,9]; 

[m,n]  =  size (ELEM); 

q, _ 

o 

Tprime2  =  Tprime; 

Aprime  =  A ( : , ELEM) ; 

Rprime  =  R; 

CnstSA  =  [Aprime ( 7  ,:) ;  Aprime(8,:);  zeros (l,n)]; 

Cnst3R  =  [Rprime ( 7 ,:) ;  Rprime(8,:);  zeros (1,6)]; 

Cnst3T  =  [Tprime2(7);  Tprime2(8);  0]; 

Aprime (4 : 6, : )  =  Aprime (4 : 6, : )  +  DCM  23*Cnst3A; 

Rprime (4 : 6, : )  =  Rprime (4 : 6, : )  +  DCM  23*Cnst3R; 

Tprime2(4:6)  =  Tprime2(4:6)  +  DCM  23*Cnst3T; 

Cnst2A  =  [Aprime ( 4 ,:) ;  Aprime(5,:);  zeros ( 1 , n) ;] ; 

Cnst2R  =  [Rprime ( 4 ,:) ;  Rprime(5,;);  zeros (1,6)]; 

Cnst2T  =  [Tprime2(4);  Tprime2(5);  0]; 

Aprime ( 1 : 3 ,: )  =  Aprime ( 1 : 3 ,: )  +  DCM  12*Cnst2A; 

Rprime ( 1 : 3 ,: )  =  Rprime ( 1 : 3 ,: )  +  DCM  12*Cnst2R; 

Tprime2(l:3)  =  Tprime2(l:3)  +  DCM  12*Cnst2T; 

Aprime  =  Aprime (ELEM,  : )  ; 

Rprime  =  Rprime (ELEM, : ) ; 

Sprime  =  S ( : , ELEM) ; 

Tprime2  =  Tprime2 (ELEM) ; 

alpha2  =  Rprime*eye ( 6) /U; 
right  =  Tprime2  -  alpha2*Fprime; 
left  =  Aprime  -  alpha2*Sprime; 

global  xdot  xdotprime 
xdot  =  leftXright; 


g,  g, 
o  o 

syms  xlx  xly  xlz 
syms  vlx  vly  viz 

xdotempty  =  sym( 'xdotempty' , [12  1] ) ; 
xdotempty ( 1 : 12 )  =  zeros (12,1); 
for  i  =  1 : n 

j  =  ELEM(i) ; 
xdotempty (j)  =  xdot(i); 

end 

%xdotprime  =  [xdotempty; ulx; uly; ul z ; u2x; u2y; u2 z ; vlx; vly; vl z ] ; 
%expanded  xdot  to  include  total  states  to  be  solved  in  ODE45 
xdotprime  =  [xdotempty ( 1 : 12 ) ;  %will  yield  wl,  ul,  u2,  vl 
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ulx; 

%wi 

11 

yield 

tlx 

uly; 

%wi 

11 

yield 

tiy 

ul  z  ; 

%wi 

11 

yield 

tl  z 

u2x; 

%wi 

11 

yield 

t2x 

u2y; 

%wi 

11 

yield 

t2y 

u2  z  ; 

%wi 

11 

yield 

t2  z 

vlx; 

%wi 

11 

yield 

dlx 

vly; 

%wi 

11 

yield 

dly 

vl  z ; 

%wi 

11 

yield 

dl  z 

qdot; 

] ; 

%wi 

11 

yield 

q 

xdotprime  =  simplify (xdotprime) ; 


% STARTING 
tf  =  10; 
wlxinit  = 
ulxinit  = 
u2xinit  = 
vlxinit  = 
tlxinit  = 
t2xinit  = 
dlxinit  = 
qlinit  =  0; 


wlyinit  = 
ulyinit  = 
u2yinit  = 
vlyinit  = 
tlyinit  = 
t2yinit  = 
dlyinit  = 
q2init  =  0 


0;  wlzinit 
0;  ulzinit 
0;  u2zinit 
0;  vlzinit 
0;  tlzinit 
0;  t2zinit 
0;  dlzinit 
;  qSinit 


0; 

0; 

0; 

0; 

0; 

0; 

0; 

0;  qiinit 


CONDITIONS 

0 
0 
0 
0 
0 
0 
0 


1 


xinit  =  [ 

wlxinit; 

wlyinit; 

wlzinit; 

ulxinit; 

ulyinit; 

ulzinit; 

u2xinit; 

u2yinit; 

u2zinit; 

vlxinit; 

vlyinit; 

vlzinit; 

tlxinit; 

tlyinit; 

tlzinit; 

t2xinit; 

t2yinit; 

t2zinit; 

dlxinit; 

dlyinit; 

dlzinit; 

qlinit; 

q2init; 

qSinit; 

q4init] ; 
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options  =  odeset ( 'RelTol' , le-6, ' AbsTol' , le-9) ; 
[time, response]  =  ode45 (@dynamics,  [0  tf ] ,  xin 


wlx  ans  =  response  ( 

,1)  ; 

wly  ans  =  response  ( 

,2)  ; 

wlz  ans  =  response  ( 

,3)  ; 

ulx  ans  =  response ( 

,4)  ; 

uly  ans  =  response ( 

,  5)  ; 

ulz  ans  =  response  ( 

,  6)  ; 

u2x  ans  =  response ( 

,7)  ; 

u2y  ans  =  response  ( 

,  8)  ; 

u2z  ans  =  response ( 

,  9)  ; 

vlx  ans  =  response  ( 

,  10)  ; 

vly  ans  =  response  ( 

,  11)  ; 

viz  ans  =  response  ( 

,  12)  ; 

tlx  ans  =  response  ( 

,  13)  ; 

tly  ans  =  response ( 

,  14)  ; 

tlz  ans  =  response  ( 

,  15)  ; 

t2x  ans  =  response ( 

,  16)  ; 

t2y  ans  =  response  ( 

,  17)  ; 

t2z  ans  =  response ( 

,  18)  ; 

dlx  ans  =  response ( 

,  19)  ; 

dly  ans  =  response ( 

,20)  ; 

dlz  ans  =  response ( 

,21)  ; 

ql_ans  =  response (:, 22 ) ; 
q2_ans  =  response (:,  23)  ; 
q3_ans  =  response (:, 24 ) ; 
q4_ans  =  response (:, 25) ; 

figure 

subplot  (2,2, 1 ) 

plot (time, tly  ans* (180/pi) , 'm' ,  'LineWidth' , 2 ) 
grid  on 

title ( 'Gimbal  1  angle') 
ylabel ( 'Angle  (deg) ' ) 
xlabel ( 'Time  (s) ' ) 

subplot  (2,2,2) 

plot (time, uly  ans* (180/pi) , 'm' ,  'LineWidth' , 2 ) 
grid  on 

title ( 'Gimbal  1  rate') 
ylabel ( 'Angle  (deg/s) ' ) 
xlabel ( 'Time  (s) ' ) 

subplot (2,2,3) 

plot (time, t2z  ans* ( 1 8 0/pi ) , ' b' ,  'LineWidth' , 2 ) 
grid  on 

title ( 'Gimbal  2  angle') 
ylabel ( 'Angle  (deg) ' ) 
xlabel ( 'Time  (s) ' ) 

subplot (2,2,4) 

plot (time, u2z  ans* ( 1 8 0/pi ) , ' b' ,  'LineWidth' , 2 ) 
grid  on 

title ( 'Gimbal  2  rate') 
ylabel ( 'Angle  (deg) ' ) 


it,  options) 
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xlabel ( 'Time  (s) ' ) 


save  three_body_data .mat; 


end 


function  [dx]  =  dynamics (t, y) 

global  xdotprime 

wlx  =  y ( 1 )  ; 

wly  =  y (2) ; 

wl z  =  y  ( 3 ) ; 

ulx  =  y (4) ; 

uly  =  y (5) ; 

ulz  =  y (6) ; 

u2x  =  y (7) ; 

u2  y  =  y  ( 8 )  ; 

u2  z  =  y  ( 9 )  ; 

vlx  =  y ( 1 0 ) ; 

vly  =  y (11) ; 

viz  =  y (12) ; 

tlx  =  y (13) ; 

tly  =  y(14) ; 

tlz  =  y (15) ; 

t2x  =  y (16) ; 

t2y  =  y (17) ; 

t2  z  =  y  ( 1 8 )  ; 

dlx  =  y(19) ; 

dly  =  y (20) ; 

dlz  =  y (21) ; 

ql  =  y (22) ; 

q2  =  y  ( 2  3 )  ; 

q3  =  y  ( 2  4  )  ; 

q4  =  y  ( 2  5 )  ; 

%assings  input  values  to  state  variables 


qdot  =  . 5* [q4  , 

-q3. 

q2. 

ql; 

q3  , 

q4. 

-ql. 

q2; 

-q2. 

ql. 

q4. 

q3; 

-ql. 

-q2. 

-q3. 

q4 ] * [wlx; wly; wl z ; 0 ] ; 

^calculate  quaternion 
^Bong  Wie  pg  344 


TROC  based  on  quaternions 


and  wl 


dx  =  [eval (xdotprime) ] ;  %calculates  and  returns  TROCs 
t 

end 
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